Showing preview only (1,665K chars total). Download the full file or copy to clipboard to get everything.
Repository: gtrll/gpslam
Branch: master
Commit: 2c56dbac000a
Files: 64
Total size: 1.6 MB
Directory structure:
gitextract_xmqbx817/
├── .gitignore
├── CHANGELOG
├── CMakeLists.txt
├── LICENSE
├── README.md
├── gpslam/
│ ├── CMakeLists.txt
│ ├── gp/
│ │ ├── CMakeLists.txt
│ │ ├── GPutils.cpp
│ │ ├── GPutils.h
│ │ ├── GaussianProcessInterpolatorLinear.h
│ │ ├── GaussianProcessInterpolatorPose2.h
│ │ ├── GaussianProcessInterpolatorPose3.h
│ │ ├── GaussianProcessInterpolatorPose3VW.h
│ │ ├── GaussianProcessInterpolatorRot3.h
│ │ ├── GaussianProcessPriorLinear.h
│ │ ├── GaussianProcessPriorPose2.h
│ │ ├── GaussianProcessPriorPose3.h
│ │ ├── GaussianProcessPriorPose3VW.h
│ │ ├── GaussianProcessPriorRot3.h
│ │ ├── Pose3utils.cpp
│ │ ├── Pose3utils.h
│ │ └── tests/
│ │ ├── testGaussianProcessInterpolatorLinear.cpp
│ │ ├── testGaussianProcessInterpolatorPose2.cpp
│ │ ├── testGaussianProcessInterpolatorPose3.cpp
│ │ ├── testGaussianProcessInterpolatorPose3VW.cpp
│ │ ├── testGaussianProcessInterpolatorRot3.cpp
│ │ ├── testGaussianProcessPriorLinear.cpp
│ │ ├── testGaussianProcessPriorPose2.cpp
│ │ ├── testGaussianProcessPriorPose3.cpp
│ │ ├── testGaussianProcessPriorPose3VW.cpp
│ │ ├── testGaussianProcessPriorRot3.cpp
│ │ ├── testPose3Utils.cpp
│ │ └── testSerializationGP.cpp
│ └── slam/
│ ├── CMakeLists.txt
│ ├── GPInterpolatedAttitudeFactorRot3.h
│ ├── GPInterpolatedGPSFactorPose3.h
│ ├── GPInterpolatedGPSFactorPose3VW.h
│ ├── GPInterpolatedProjectionFactorPose3.h
│ ├── GPInterpolatedRangeFactor2DLinear.h
│ ├── GPInterpolatedRangeFactorPose2.h
│ ├── GPInterpolatedRangeFactorPose3.h
│ ├── OdometryFactor2DLinear.h
│ ├── RangeBearingFactor2DLinear.h
│ ├── RangeFactor2DLinear.h
│ ├── RangeFactorPose2.h
│ └── tests/
│ ├── testGPInterpolatedGPSFactorPose3.cpp
│ ├── testGPInterpolatedGPSFactorPose3VW.cpp
│ ├── testGPInterpolatedProjectionFactorPose3.cpp
│ ├── testGPInterpolatedRangeFactor2DLinear.cpp
│ ├── testGPInterpolatedRangeFactorPose2.cpp
│ ├── testGPInterpolatedRangeFactorPose3.cpp
│ ├── testOdometryFactor2DLinear.cpp
│ ├── testRangeBearingFactor2DLinear.cpp
│ ├── testRangeFactor2DLinear.cpp
│ └── testSerializationSLAM.cpp
├── gpslam.h
└── matlab/
├── AHRSexample.m
├── GPAHRSexample.m
├── PlazaPose2.m
├── data/
│ ├── MOCAP_POSE_DATA_matlab.txt
│ ├── Plaza1.mat
│ ├── Plaza2.mat
│ └── RAW_IMU_DATA_matlab.txt
└── range_measure_fit.m
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
*.DS_Store
build*
================================================
FILE: CHANGELOG
================================================
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for GP-SLAM
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.0 (2017-08-25)
------------------
* Initial release
* Contributors: Jing Dong, Xinyan Yan
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.6)
enable_testing()
project(gpslam CXX C)
# version indicator
set(GPSLAM_VERSION_MAJOR 0)
set(GPSLAM_VERSION_MINOR 1)
set(GPSLAM_VERSION_PATCH 0)
set(GPSLAM_VERSION_STRING "${GPSLAM_VERSION_MAJOR}.${GPSLAM_VERSION_MINOR}.${GPSLAM_VERSION_PATCH}")
# Find dependent libraries
# Find GTSAM components
find_package(GTSAM REQUIRED) # Uses installed package
include_directories(${GTSAM_INCLUDE_DIR})
set(GTSAM_LIBS gtsam)
find_package(GTSAMCMakeTools)
include(GtsamMakeConfigFile)
include(GtsamBuildTypes)
include(GtsamTesting)
# options
option(GPSLAM_BUILD_MATLAB_TOOLBOX "whether build matlab toolbox" OFF)
# for unittest scripts
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools")
# Boost - same requirement as gtsam
find_package(Boost 1.46 REQUIRED)
find_package(Boost COMPONENTS filesystem REQUIRED)
find_package(Boost COMPONENTS system REQUIRED)
find_package(Boost COMPONENTS thread REQUIRED)
find_package(Boost COMPONENTS serialization REQUIRED)
include_directories(${Boost_INCLUDE_DIR})
# include current source folder, at the very beginning
include_directories(BEFORE ${CMAKE_CURRENT_SOURCE_DIR})
# Process source subdirs
add_subdirectory(gpslam)
# Wrapping to MATLAB
if(GPSLAM_BUILD_MATLAB_TOOLBOX)
include(GtsamMatlabWrap)
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
wrap_and_install_library(gpslam.h ${PROJECT_NAME} "${CMAKE_CURRENT_SOURCE_DIR}" "")
endif()
# Install config and export files
GtsamMakeConfigFile(gpslam)
export(TARGETS ${gpslam_EXPORTED_TARGETS} FILE gpslam-exports.cmake)
================================================
FILE: LICENSE
================================================
Copyright (c) 2016, Georgia Tech Research Corporation
Atlanta, Georgia 30332-0415
All Rights Reserved
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
================================================
FILE: README.md
================================================
GP-SLAM
===================================================
GP-SLAM is a library implenmenting sparse Gaussian process (GP) regression for continuous-time trajectory estimation and mapping. The core library is developed by C++ language, and an optional Matlab toolbox is also provided. Examples are provided in Matlab scripts.
GP-SLAM is being developed by [Jing Dong](mailto:thu.dongjing@gmail.com) and [Xinyan Yan](mailto:voidpointeryan@gmail.com) as part of their work at Georgia Tech Robot Learning Lab.
Prerequisites
------
- CMake >= 2.6 (Ubuntu: `sudo apt-get install cmake`), compilation configuration tool.
- [Boost](http://www.boost.org/) >= 1.46 (Ubuntu: `sudo apt-get install libboost-all-dev`), portable C++ source libraries.
- [GTSAM](https://bitbucket.org/gtborg/gtsam) >= 4.0 alpha, a C++ library that implement smoothing and mapping (SAM) in robotics and vision.
Compilation & Installation
------
In the library folder excute:
```
$ mkdir build
$ cd build
$ cmake ..
$ make check # optonal, run unit tests
$ make install
```
Matlab Toolbox
-----
An optional Matlab toolbox is provided to use our library in Matlab. To enable Matlab toolbox during compilation:
```
$ cmake -DGPSLAM_BUILD_MATLAB_TOOLBOX:OPTION=ON -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=/path/install/toolbox ..
$ make install
```
After you install the Matlab toolbox, don't forget to add your `/path/install/toolbox` to your Matlab path.
Compatibility
-----
The GP-SLAM library is designed to be cross-platform, but it has been only tested on Ubuntu Linux for now.
Tested Compilers:
- GCC 4.8, 5.4
Tested Boost version: 1.48-1.61
Linking to External Projects
-----
We provide easy linking to external **CMake** projects. Add following lines to your CMakeLists.txt
```
find_package(gpslam REQUIRED)
include_directories(${gpslam_INCLUDE_DIR})
```
Questions & Bug reporting
-----
Please use Github issue tracker to report bugs. For other questions please contact [Jing Dong](mailto:thu.dongjing@gmail.com).
Citing
-----
If you use GP-SLAM in an academic context, please cite following publications:
```
@inproceedings{Yan17ras,
Author = "Xinyan Yan and Vadim Indelman and Byron Boots",
journal = " Robotics and Autonomous Systems",
Title = "Incremental Sparse {GP} Regression for Continuous-time Trajectory Estimation and Mapping",
Year = {2017},
pages="120-132",
volume = {87}
}
@article{Dong17arxiv,
author = {Jing Dong and Byron Boots and Frank Dellaert},
title = {Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups},
journal = {Arxiv},
volume = {abs/1705.06020},
year = {2017},
url = {http://arxiv.org/abs/1705.06020}
}
```
License
-----
GP-SLAM is released under the BSD license, reproduced in the file LICENSE in this directory.
================================================
FILE: gpslam/CMakeLists.txt
================================================
# We split the library in to separate subfolders, each containing
# tests, timing, and an optional convenience library.
# The following variable is the master list of subdirs to add
set(gpslam_subdirs
gp
slam
)
set(gpslam_srcs)
# files want to be excluded
set(excluded_sources "")
# Library sources
foreach(subdir ${gpslam_subdirs})
file(GLOB subdir_srcs "${subdir}/*.cpp" "${subdir}/*.h")
list(REMOVE_ITEM subdir_srcs "${excluded_sources}")
file(GLOB subdir_test_files "${subdir}/tests/*")
list(APPEND gpslam_srcs ${subdir_srcs})
message(STATUS "Building Module: ${subdir}")
# local and tests
add_subdirectory(${subdir})
endforeach(subdir)
# build shared lib
add_library(${PROJECT_NAME} SHARED ${gpslam_srcs})
target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${GTSAM_LIBS})
# Install library
install(TARGETS ${PROJECT_NAME} EXPORT gpslam-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
================================================
FILE: gpslam/gp/CMakeLists.txt
================================================
# Install headers
file(GLOB gp_headers "*.h")
install(FILES ${gp_headers} DESTINATION include/gpslam/gp)
# Build tests
gtsamAddTestsGlob(gp "tests/*.cpp" "" ${PROJECT_NAME})
================================================
FILE: gpslam/gp/GPutils.cpp
================================================
/**
* @file GPutils.cpp
* @brief GP utils, calculation of Qc, Q, Lamda matrices etc.
* @author Xinyan Yan, Jing Dong
* @date Qct 26, 2015
**/
#include <gpslam/gp/GPutils.h>
using namespace gtsam;
namespace gpslam {
/* ************************************************************************** */
Matrix getQc(const SharedNoiseModel& Qc_model) {
noiseModel::Gaussian *Gassian_model =
dynamic_cast<noiseModel::Gaussian*>(Qc_model.get());
return (Gassian_model->R().transpose() * Gassian_model->R()).inverse();
}
} // namespace gtsam
================================================
FILE: gpslam/gp/GPutils.h
================================================
/**
* @file GPutils.h
* @brief GP utils, calculation of Qc, Q, Lamda matrices etc.
* @author Xinyan Yan, Jing Dong
* @date Qct 26, 2015
**/
#pragma once
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <cmath>
namespace gpslam {
/// get Qc covariance matrix from noise model
gtsam::Matrix getQc(const gtsam::SharedNoiseModel& Qc_model);
/// calculate Q
template <int Dim>
Eigen::Matrix<double, 2*Dim, 2*Dim> calcQ(const Eigen::Matrix<double, Dim, Dim>& Qc, double tau) {
Eigen::Matrix<double, 2*Dim, 2*Dim> Q = (Eigen::Matrix<double, 2*Dim, 2*Dim>() <<
1.0 / 3 * pow(tau, 3.0) * Qc, 1.0 / 2 * pow(tau, 2.0) * Qc,
1.0 / 2 * pow(tau, 2.0) * Qc, tau * Qc).finished();
return Q;
}
/// calculate Q_inv
template <int Dim>
Eigen::Matrix<double, 2*Dim, 2*Dim> calcQ_inv(const Eigen::Matrix<double, Dim, Dim>& Qc, double tau) {
Eigen::Matrix<double, Dim, Dim> Qc_inv = Qc.inverse();
Eigen::Matrix<double, 2*Dim, 2*Dim> Q_inv =
(Eigen::Matrix<double, 2*Dim, 2*Dim>() <<
12.0 * pow(tau, -3.0) * Qc_inv, (-6.0) * pow(tau, -2.0) * Qc_inv,
(-6.0) * pow(tau, -2.0) * Qc_inv, 4.0 * pow(tau, -1.0) * Qc_inv).finished();
return Q_inv;
}
/// calculate Phi
template <int Dim>
Eigen::Matrix<double, 2*Dim, 2*Dim> calcPhi(double tau) {
Eigen::Matrix<double, 2*Dim, 2*Dim> Phi = (Eigen::Matrix<double, 2*Dim, 2*Dim>() <<
Eigen::Matrix<double, Dim, Dim>::Identity(), tau * Eigen::Matrix<double, Dim, Dim>::Identity(),
Eigen::Matrix<double, Dim, Dim>::Zero(), Eigen::Matrix<double, Dim, Dim>::Identity()).finished();
return Phi;
}
/// calculate Lambda
template <int Dim>
Eigen::Matrix<double, 2*Dim, 2*Dim> calcLambda(const Eigen::Matrix<double, Dim, Dim>& Qc,
double delta_t, const double tau) {
Eigen::Matrix<double, 2*Dim, 2*Dim> Lambda = calcPhi<Dim>(tau) - calcQ(Qc, tau) * (calcPhi<Dim>(delta_t - tau).transpose())
* calcQ_inv(Qc, delta_t) * calcPhi<Dim>(delta_t);
return Lambda;
}
/// calculate Psi
template <int Dim>
Eigen::Matrix<double, 2*Dim, 2*Dim> calcPsi(const Eigen::Matrix<double, Dim, Dim>& Qc,
double delta_t, double tau) {
Eigen::Matrix<double, 2*Dim, 2*Dim> Psi = calcQ(Qc, tau) * (calcPhi<Dim>(delta_t - tau).transpose())
* calcQ_inv(Qc, delta_t);
return Psi;
}
} // namespace gtsam
================================================
FILE: gpslam/gp/GaussianProcessInterpolatorLinear.h
================================================
/**
* @file GaussianProcessInterpolatorLinear.h
* @brief Base and utils for Gaussian Process Interpolated measurement factor, linear version
* @author Jing Dong, Xinyan Yan
* @date Oct 26, 2015
*/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Matrix.h>
#include <boost/serialization/array.hpp>
namespace gpslam {
/**
* 4-way factor for Gaussian Process interpolator, linear version
* interpolate pose and velocity given consecutive poses and velocities
*/
template <int Dim>
class GaussianProcessInterpolatorLinear {
private:
typedef GaussianProcessInterpolatorLinear<Dim> This;
typedef Eigen::Matrix<double, Dim, 1> VectorDim;
typedef Eigen::Matrix<double, 2*Dim, 1> Vector2Dim;
typedef Eigen::Matrix<double, Dim, Dim> MatrixDim;
typedef Eigen::Matrix<double, 2*Dim, 2*Dim> Matrix2Dim;
double delta_t_; // t_{i+1} - t_i
double tau_; // tau - t_i. we use tau as time interval from t_i instead of from t_0 as in Barfoot papers
MatrixDim Qc_;
Matrix2Dim Lambda_;
Matrix2Dim Psi_;
public:
/// Default constructor: only for serialization
GaussianProcessInterpolatorLinear() {}
/**
* Constructor
* @param Qc noise model of Qc
* @param delta_t the time between the two states
* @param tau the time of interval status
*/
GaussianProcessInterpolatorLinear(const gtsam::SharedNoiseModel& Qc_model,
double delta_t, double tau) :
delta_t_(delta_t), tau_(tau) {
// check noise model dim
assert(Qc_model->dim() == Dim);
// Calcuate Lambda and Psi
Qc_ = getQc(Qc_model);
Lambda_ = calcLambda(Qc_, delta_t_, tau_);
Psi_ = calcPsi(Qc_, delta_t_, tau_);
}
/** Virtual destructor */
virtual ~GaussianProcessInterpolatorLinear() {}
/// interpolate pose with Jacobians
VectorDim interpolatePose(
const VectorDim& pose1, const VectorDim& vel1,
const VectorDim& pose2, const VectorDim& vel2,
gtsam::OptionalJacobian<Dim, Dim> H1 = boost::none,
gtsam::OptionalJacobian<Dim, Dim> H2 = boost::none,
gtsam::OptionalJacobian<Dim, Dim> H3 = boost::none,
gtsam::OptionalJacobian<Dim, Dim> H4 = boost::none) const {
// state vector
Vector2Dim x1 = (Vector2Dim() << pose1, vel1).finished();
Vector2Dim x2 = (Vector2Dim() << pose2, vel2).finished();
// jacobians
if (H1) *H1 = Lambda_.template block<Dim,Dim>(0, 0);
if (H2) *H2 = Lambda_.template block<Dim,Dim>(0, Dim);
if (H3) *H3 = Psi_.template block<Dim,Dim>(0, 0);
if (H4) *H4 = Psi_.template block<Dim,Dim>(0, Dim);
// interpolate pose (just calculate upper part of the interpolated state vector to save time)
return Lambda_.template block<Dim,2*Dim>(0, 0) * x1 + Psi_.template block<Dim,2*Dim>(0, 0) * x2;
}
/// update jacobian based on interpolated jacobians
static void updatePoseJacobians(const gtsam::Matrix& Hpose, const gtsam::Matrix& Hint1,
const gtsam::Matrix& Hint2, const gtsam::Matrix& Hint3, const gtsam::Matrix& Hint4,
boost::optional<gtsam::Matrix&> H1, boost::optional<gtsam::Matrix&> H2,
boost::optional<gtsam::Matrix&> H3, boost::optional<gtsam::Matrix&> H4) {
if (H1) *H1 = Hpose * Hint1;
if (H2) *H2 = Hpose * Hint2;
if (H3) *H3 = Hpose * Hint3;
if (H4) *H4 = Hpose * Hint4;
}
/// interpolate velocity with Jacobians
VectorDim interpolateVelocity(
const VectorDim& pose1, const VectorDim& vel1,
const VectorDim& pose2, const VectorDim& vel2,
gtsam::OptionalJacobian<Dim, Dim> H1 = boost::none,
gtsam::OptionalJacobian<Dim, Dim> H2 = boost::none,
gtsam::OptionalJacobian<Dim, Dim> H3 = boost::none,
gtsam::OptionalJacobian<Dim, Dim> H4 = boost::none) const {
// state vector
Vector2Dim x1 = (Vector2Dim() << pose1, vel1).finished();
Vector2Dim x2 = (Vector2Dim() << pose2, vel2).finished();
// jacobians
if (H1) *H1 = Lambda_.template block<Dim,Dim>(Dim, 0);
if (H2) *H2 = Lambda_.template block<Dim,Dim>(Dim, Dim);
if (H3) *H3 = Psi_.template block<Dim,Dim>(Dim, 0);
if (H4) *H4 = Psi_.template block<Dim,Dim>(Dim, Dim);
// interpolate pose (just calculate lower part of the interpolated state vector to save time)
return Lambda_.template block<Dim,2*Dim>(Dim, 0) * x1 + Psi_.template block<Dim,2*Dim>(Dim, 0) * x2;
}
/** demensions */
size_t dim() const { return Dim; }
/**
* Testables
*/
/** equals specialized to this factor */
virtual bool equals(const This& expected, double tol=1e-9) const {
return fabs(this->delta_t_ - expected.delta_t_) < tol &&
fabs(this->tau_ - expected.tau_) < tol &&
gtsam::equal_with_abs_tol(this->Qc_, expected.Qc_, tol) &&
gtsam::equal_with_abs_tol(this->Lambda_, expected.Lambda_, tol) &&
gtsam::equal_with_abs_tol(this->Psi_, expected.Psi_, tol);
}
/** print contents */
void print(const std::string& s="") const {
std::cout << s << "GaussianProcessInterpolatorLinear<" << Dim << ">" << std::endl;
std::cout << "delta_t = " << delta_t_ << ", tau = " << tau_ << std::endl;
//std::cout << "Qc = " << Qc_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(delta_t_);
ar & BOOST_SERIALIZATION_NVP(tau_);
using namespace boost::serialization;
ar & make_nvp("Qc", make_array(Qc_.data(), Qc_.size()));
ar & make_nvp("Lambda", make_array(Lambda_.data(), Lambda_.size()));
ar & make_nvp("Psi", make_array(Psi_.data(), Psi_.size()));
}
};
} // \ namespace gpslam
/// traits
namespace gtsam {
template<int Dim>
struct traits<gpslam::GaussianProcessInterpolatorLinear<Dim> > : public Testable<
gpslam::GaussianProcessInterpolatorLinear<Dim> > {};
}
================================================
FILE: gpslam/gp/GaussianProcessInterpolatorPose2.h
================================================
/**
* @file GaussianProcessInterpolatorPose2.h
* @brief Base and utils for Gaussian Process Interpolated measurement factor, works only in SE(2)
* @author Jing Dong
*/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Matrix.h>
namespace gpslam {
/**
* 4-way factor for Gaussian Process interpolator, SE(2) version
* interpolate pose and velocity given consecutive poses and velocities
*/
class GaussianProcessInterpolatorPose2 {
private:
typedef GaussianProcessInterpolatorPose2 This;
double delta_t_; // t_{i+1} - t_i
double tau_; // tau - t_i. we use tau as time interval from t_i instead of from t_0 as in Barfoot papers
gtsam::Matrix3 Qc_;
gtsam::Matrix6 Lambda_;
gtsam::Matrix6 Psi_;
public:
/// Default constructor: only for serialization
GaussianProcessInterpolatorPose2() {}
/**
* Constructor
* @param Qc noise model of Qc
* @param delta_t the time between the two states
* @param tau the time of interval status
*/
GaussianProcessInterpolatorPose2(const gtsam::SharedNoiseModel& Qc_model, double delta_t, double tau) :
delta_t_(delta_t), tau_(tau) {
// Calcuate Lambda and Psi
Qc_ = getQc(Qc_model);
Lambda_ = calcLambda(Qc_, delta_t_, tau_);
Psi_ = calcPsi(Qc_, delta_t_, tau_);
}
/** Virtual destructor */
virtual ~GaussianProcessInterpolatorPose2() {}
/// interpolate pose with Jacobians
gtsam::Pose2 interpolatePose(
const gtsam::Pose2& pose1, const gtsam::Vector3& vel1,
const gtsam::Pose2& pose2, const gtsam::Vector3& vel2,
gtsam::OptionalJacobian<3, 3> H1 = boost::none,
gtsam::OptionalJacobian<3, 3> H2 = boost::none,
gtsam::OptionalJacobian<3, 3> H3 = boost::none,
gtsam::OptionalJacobian<3, 3> H4 = boost::none) const {
using namespace gtsam;
const Vector6 r1 = (Vector6() << Vector3::Zero(), vel1).finished();
Matrix3 Hinv, Hcomp11, Hcomp12, Hlogmap;
Vector3 r;
if (H1 || H2 || H3 || H4)
r = Pose2::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp11, Hcomp12), Hlogmap);
else
r = Pose2::Logmap(pose1.inverse().compose(pose2));
const Vector6 r2 = (Vector6() << r, vel2).finished();
Pose2 pose;
if (H1 || H2 || H3 || H4) {
Matrix3 Hcomp21, Hcomp22, Hexp;
pose = pose1.compose(Pose2::Expmap(Lambda_.block<3, 6>(0, 0) * r1 + Psi_.block<3, 6>(0, 0) * r2, Hexp), Hcomp21, Hcomp22);
Matrix3 Hexpr1 = Hcomp22*Hexp;
if (H1) *H1 = Hcomp21 + Hexpr1*Psi_.block<3, 3>(0, 0)*Hlogmap*Hcomp11*Hinv;
if (H2) *H2 = Hexpr1*Lambda_.block<3, 3>(0, 3);
if (H3) *H3 = Hexpr1*Psi_.block<3, 3>(0, 0)*Hlogmap*Hcomp12;
if (H4) *H4 = Hexpr1*Psi_.block<3, 3>(0, 3);
} else {
pose = pose1.compose(Pose2::Expmap(Lambda_.block<3, 6>(0, 0) * r1 + Psi_.block<3, 6>(0, 0) * r2));
}
return pose;
}
/// update jacobian based on interpolated jacobians
static void updatePoseJacobians(const gtsam::Matrix& Hpose, const gtsam::Matrix& Hint1,
const gtsam::Matrix& Hint2, const gtsam::Matrix& Hint3, const gtsam::Matrix& Hint4,
boost::optional<gtsam::Matrix&> H1, boost::optional<gtsam::Matrix&> H2,
boost::optional<gtsam::Matrix&> H3, boost::optional<gtsam::Matrix&> H4) {
if (H1) *H1 = Hpose * Hint1;
if (H2) *H2 = Hpose * Hint2;
if (H3) *H3 = Hpose * Hint3;
if (H4) *H4 = Hpose * Hint4;
}
/// interpolate velocity with Jacobians
/// TODO: implementation
gtsam::Vector3 interpolateVelocity(
const gtsam::Pose2& pose1, const gtsam::Vector3& vel1,
const gtsam::Pose2& pose2, const gtsam::Vector3& vel2,
gtsam::OptionalJacobian<3, 3> H1 = boost::none,
gtsam::OptionalJacobian<3, 3> H2 = boost::none,
gtsam::OptionalJacobian<3, 3> H3 = boost::none,
gtsam::OptionalJacobian<3, 3> H4 = boost::none) const ;
/**
* Testables
*/
/** equals specialized to this factor */
virtual bool equals(const This& expected, double tol=1e-9) const {
return fabs(this->delta_t_ - expected.delta_t_) < tol &&
fabs(this->tau_ - expected.tau_) < tol &&
gtsam::equal_with_abs_tol(this->Qc_, expected.Qc_, tol) &&
gtsam::equal_with_abs_tol(this->Lambda_, expected.Lambda_, tol) &&
gtsam::equal_with_abs_tol(this->Psi_, expected.Psi_, tol);
}
/** print contents */
void print(const std::string& s="") const {
std::cout << s << "GaussianProcessInterpolatorPose2" << std::endl;
std::cout << "delta_t = " << delta_t_ << ", tau = " << tau_ << std::endl;
//std::cout << "Qc = " << Qc_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(delta_t_);
ar & BOOST_SERIALIZATION_NVP(tau_);
using namespace boost::serialization;
ar & make_nvp("Qc", make_array(Qc_.data(), Qc_.size()));
ar & make_nvp("Lambda", make_array(Lambda_.data(), Lambda_.size()));
ar & make_nvp("Psi", make_array(Psi_.data(), Psi_.size()));
}
};
} // \ namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessInterpolatorPose2> : public Testable<
gpslam::GaussianProcessInterpolatorPose2> {};
}
================================================
FILE: gpslam/gp/GaussianProcessInterpolatorPose3.h
================================================
/**
* @file GaussianProcessInterpolatorPose3.h
* @brief Base and utils for Gaussian Process Interpolated measurement factor, SE(3) version
* @author Jing Dong, Xinyan Yan
*/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gpslam/gp/Pose3utils.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
namespace gpslam {
/**
* 4-way factor for Gaussian Process interpolator, SE(3) version
* interpolate pose and velocity given consecutive poses and velocities
*/
class GaussianProcessInterpolatorPose3 {
private:
typedef GaussianProcessInterpolatorPose3 This;
double delta_t_; // t_{i+1} - t_i
double tau_; // tau - t_i. we use tau as time interval from t_i instead of from t_0 as in Barfoot papers
gtsam::Matrix6 Qc_;
Matrix_12 Lambda_;
Matrix_12 Psi_;
public:
/// Default constructor: only for serialization
GaussianProcessInterpolatorPose3() {}
/**
* Constructor
* @param Qc noise model of Qc
* @param delta_t the time between the two states
* @param tau the time of interval status
*/
GaussianProcessInterpolatorPose3(const gtsam::SharedNoiseModel& Qc_model, double delta_t, double tau) :
delta_t_(delta_t), tau_(tau) {
// Calcuate Lambda and Psi
Qc_ = getQc(Qc_model);
Lambda_ = calcLambda(Qc_, delta_t_, tau_);
Psi_ = calcPsi(Qc_, delta_t_, tau_);
}
/** Virtual destructor */
virtual ~GaussianProcessInterpolatorPose3() {}
/// interpolate pose with Jacobians
gtsam::Pose3 interpolatePose(const gtsam::Pose3& pose1, const gtsam::Vector6& vel1,
const gtsam::Pose3& pose2, const gtsam::Vector6& vel2,
gtsam::OptionalJacobian<6, 6> H1 = boost::none, gtsam::OptionalJacobian<6, 6> H2 = boost::none,
gtsam::OptionalJacobian<6, 6> H3 = boost::none, gtsam::OptionalJacobian<6, 6> H4 = boost::none) const {
using namespace gtsam;
const Vector_12 r1 = (Vector_12() << Vector6::Zero(), vel1).finished();
Matrix6 Hinv, Hcomp11, Hcomp12, Hlogmap;
Vector6 r;
if (H1 || H2 || H3 || H4)
r = Pose3::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp11, Hcomp12), Hlogmap);
else
r = Pose3::Logmap(pose1.inverse().compose(pose2));
const Matrix6 Jinv = rightJacobianPose3inv(r);
const Vector_12 r2 = (Vector_12() << r, Jinv * vel2).finished();
Pose3 pose;
if (H1 || H2 || H3 || H4) {
Matrix6 Hcomp21, Hcomp22, Hexp;
pose = pose1.compose(Pose3::Expmap(Lambda_.block<6, 12>(0, 0) * r1 + Psi_.block<6, 12>(0, 0) * r2, Hexp), Hcomp21, Hcomp22);
Matrix6 Hexpr1 = Hcomp22*Hexp;
if (H1) {
const Matrix6 tmp = Hlogmap * Hcomp11 * Hinv;
const Matrix_12_6 dr2_dT1 = (Matrix_12_6() << tmp, jacobianMethodNumercialDiff(
rightJacobianPose3inv, r, vel2) * tmp).finished();
*H1 = Hcomp21 + Hexpr1*Psi_.block<6, 12>(0, 0)*dr2_dT1;
}
if (H2) *H2 = Hexpr1*Lambda_.block<6, 6>(0, 6);
if (H3) {
const Matrix6 tmp = Hlogmap * Hcomp12;
const Matrix_12_6 dr2_dT2 = (Matrix_12_6() << tmp, jacobianMethodNumercialDiff(
rightJacobianPose3inv, r, vel2) * tmp).finished();
*H3 = Hexpr1*Psi_.block<6, 12>(0, 0)*dr2_dT2;
}
if (H4) *H4 = Hexpr1*Psi_.block<6, 6>(0, 6) * Jinv;
} else {
pose = pose1.compose(Pose3::Expmap(Lambda_.block<6, 12>(0, 0) * r1 + Psi_.block<6, 12>(0, 0) * r2));
}
return pose;
}
/// update jacobian based on interpolated jacobians
static void updatePoseJacobians(const gtsam::Matrix& Hpose, const gtsam::Matrix& Hint1,
const gtsam::Matrix& Hint2, const gtsam::Matrix& Hint3, const gtsam::Matrix& Hint4,
boost::optional<gtsam::Matrix&> H1, boost::optional<gtsam::Matrix&> H2,
boost::optional<gtsam::Matrix&> H3, boost::optional<gtsam::Matrix&> H4) {
if (H1) *H1 = Hpose * Hint1;
if (H2) *H2 = Hpose * Hint2;
if (H3) *H3 = Hpose * Hint3;
if (H4) *H4 = Hpose * Hint4;
}
/// interpolate velocity with Jacobians
/// TODO: implementation
gtsam::Pose3 interpolateVelocity(const gtsam::Pose3& pose1, const gtsam::Vector6& vel1,
const gtsam::Pose3& pose2, const gtsam::Vector6& vel2,
gtsam::OptionalJacobian<6, 6> H1 = boost::none, gtsam::OptionalJacobian<6, 6> H2 = boost::none,
gtsam::OptionalJacobian<6, 6> H3 = boost::none, gtsam::OptionalJacobian<6, 6> H4 = boost::none) const ;
/**
* Testables
*/
/** equals specialized to this factor */
virtual bool equals(const This& expected, double tol=1e-9) const {
return fabs(this->delta_t_ - expected.delta_t_) < tol &&
fabs(this->tau_ - expected.tau_) < tol &&
gtsam::equal_with_abs_tol(this->Qc_, expected.Qc_, tol) &&
gtsam::equal_with_abs_tol(this->Lambda_, expected.Lambda_, tol) &&
gtsam::equal_with_abs_tol(this->Psi_, expected.Psi_, tol);
}
/** print contents */
void print(const std::string& s="") const {
std::cout << s << "GaussianProcessInterpolatorPose3" << std::endl;
std::cout << "delta_t = " << delta_t_ << ", tau = " << tau_ << std::endl;
//std::cout << "Qc = " << Qc_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(delta_t_);
ar & BOOST_SERIALIZATION_NVP(tau_);
using namespace boost::serialization;
ar & make_nvp("Qc", make_array(Qc_.data(), Qc_.size()));
ar & make_nvp("Lambda", make_array(Lambda_.data(), Lambda_.size()));
ar & make_nvp("Psi", make_array(Psi_.data(), Psi_.size()));
}
};
} // \ namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessInterpolatorPose3> : public Testable<
gpslam::GaussianProcessInterpolatorPose3> {};
}
================================================
FILE: gpslam/gp/GaussianProcessInterpolatorPose3VW.h
================================================
/**
* @file GaussianProcessInterpolatorPose3VW.h
* @brief Base and utils for Gaussian Process Interpolated measurement factor, SE(3). use V/W velocity representation
* @author Jing Dong, Xinyan Yan
*/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gpslam/gp/Pose3utils.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
namespace gpslam {
/**
* 4-way factor for Gaussian Process interpolator, SE(3) version
* 6-DOF velocity is represented by 3-DOF translational and 3-DOF rotational velocities (in body frame).
* interpolate pose and velocity given consecutive poses and velocities
*/
class GaussianProcessInterpolatorPose3VW {
private:
typedef GaussianProcessInterpolatorPose3VW This;
double delta_t_; // t_{i+1} - t_i
double tau_; // tau - t_i. we use tau as time interval from t_i instead of from t_0 as in Barfoot papers
gtsam::Matrix6 Qc_;
Matrix_12 Lambda_;
Matrix_12 Psi_;
public:
/// Default constructor: only for serialization
GaussianProcessInterpolatorPose3VW() {}
/**
* Constructor
* @param Qc noise model of Qc
* @param delta_t the time between the two states
* @param tau the time of interval status
*/
GaussianProcessInterpolatorPose3VW(const gtsam::SharedNoiseModel& Qc_model, double delta_t, double tau) :
delta_t_(delta_t), tau_(tau) {
// Calcuate Lambda and Psi
Qc_ = getQc(Qc_model);
Lambda_ = calcLambda(Qc_, delta_t_, tau_);
Psi_ = calcPsi(Qc_, delta_t_, tau_);
}
/** Virtual destructor */
virtual ~GaussianProcessInterpolatorPose3VW() {}
/// interpolate pose with Jacobians
gtsam::Pose3 interpolatePose(
const gtsam::Pose3& pose1, const gtsam::Vector3& v1, const gtsam::Vector3& omega1,
const gtsam::Pose3& pose2, const gtsam::Vector3& v2, const gtsam::Vector3& omega2,
gtsam::OptionalJacobian<6, 6> H1 = boost::none, gtsam::OptionalJacobian<6, 3> H2 = boost::none,
gtsam::OptionalJacobian<6, 3> H3 = boost::none, gtsam::OptionalJacobian<6, 6> H4 = boost::none,
gtsam::OptionalJacobian<6, 3> H5 = boost::none, gtsam::OptionalJacobian<6, 3> H6 = boost::none) const {
using namespace gtsam;
Matrix6 Hinv, Hcomp11, Hcomp12, Hlogmap;
Vector6 r;
if (H1 || H4)
r = Pose3::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp11, Hcomp12), Hlogmap);
else
r = Pose3::Logmap(pose1.inverse().compose(pose2));
const Matrix6 Jinv = rightJacobianPose3inv(r);
// vel
Matrix63 H1v, H1w, H2v, H2w;
Matrix6 H1p, H2p;
Vector6 vel1, vel2;
if (H2 || H3 || H5 || H6) {
vel1 = convertVWtoVb(v1, omega1, pose1, H1v, H1w, H1p);
vel2 = convertVWtoVb(v2, omega2, pose2, H2v, H2w, H2p);
} else {
vel1 = convertVWtoVb(v1, omega1, pose1);
vel2 = convertVWtoVb(v2, omega2, pose2);
}
const Vector_12 r1 = (Vector_12() << Vector6::Zero(), vel1).finished();
const Vector_12 r2 = (Vector_12() << r, Jinv * vel2).finished();
// pose
Pose3 pose;
if (H1 || H2 || H3 || H4) {
Matrix6 Hcomp21, Hcomp22, Hexp;
pose = pose1.compose(Pose3::Expmap(Lambda_.block<6, 12>(0, 0) * r1 + Psi_.block<6, 12>(0, 0) * r2, Hexp), Hcomp21, Hcomp22);
Matrix6 Hexpr1 = Hcomp22*Hexp;
Matrix6 Hvel1 = Hexpr1*Lambda_.block<6, 6>(0, 6);
Matrix6 Hvel2 = Hexpr1*Psi_.block<6, 6>(0, 6) * Jinv;
if (H1) {
const Matrix6 tmp = Hlogmap * Hcomp11 * Hinv;
const Matrix_12_6 dr2_dT1 = (Matrix_12_6() << tmp, jacobianMethodNumercialDiff(
rightJacobianPose3inv, r, vel2) * tmp).finished();
*H1 = Hcomp21 + Hexpr1*Psi_.block<6, 12>(0, 0)*dr2_dT1 + Hvel1*H1p;
}
if (H2) *H2 = Hvel1 * H1v;
if (H3) *H3 = Hvel1 * H1w;
if (H4) {
const Matrix6 tmp = Hlogmap * Hcomp12;
const Matrix_12_6 dr2_dT2 = (Matrix_12_6() << tmp, jacobianMethodNumercialDiff(
rightJacobianPose3inv, r, vel2) * tmp).finished();
*H4 = Hexpr1*Psi_.block<6, 12>(0, 0)*dr2_dT2 + Hvel2*H2p;
}
if (H5) *H5 = Hvel2 * H2v;
if (H6) *H6 = Hvel2 * H2w;
} else {
pose = pose1.compose(Pose3::Expmap(Lambda_.block<6, 12>(0, 0) * r1 + Psi_.block<6, 12>(0, 0) * r2));
}
return pose;
}
/// update jacobian based on interpolated jacobians
static void updatePoseJacobians(const gtsam::Matrix& Hpose,
const gtsam::Matrix& Hint1, const gtsam::Matrix& Hint2, const gtsam::Matrix& Hint3,
const gtsam::Matrix& Hint4, const gtsam::Matrix& Hint5, const gtsam::Matrix& Hint6,
boost::optional<gtsam::Matrix&> H1, boost::optional<gtsam::Matrix&> H2,
boost::optional<gtsam::Matrix&> H3, boost::optional<gtsam::Matrix&> H4,
boost::optional<gtsam::Matrix&> H5, boost::optional<gtsam::Matrix&> H6) {
if (H1) *H1 = Hpose * Hint1;
if (H2) *H2 = Hpose * Hint2;
if (H3) *H3 = Hpose * Hint3;
if (H4) *H4 = Hpose * Hint4;
if (H5) *H5 = Hpose * Hint5;
if (H6) *H6 = Hpose * Hint6;
}
/// interpolate velocity with Jacobians
/// TODO: implementation
gtsam::Vector6 interpolateVelocity(
const gtsam::Pose3& pose1, const gtsam::Vector3& v1, const gtsam::Vector3& omega1,
const gtsam::Pose3& pose2, const gtsam::Vector3& v2, const gtsam::Vector3& omega2,
gtsam::OptionalJacobian<6, 6> H1 = boost::none, gtsam::OptionalJacobian<6, 3> H2 = boost::none,
gtsam::OptionalJacobian<6, 3> H3 = boost::none, gtsam::OptionalJacobian<6, 6> H4 = boost::none,
gtsam::OptionalJacobian<6, 3> H5 = boost::none, gtsam::OptionalJacobian<6, 3> H6 = boost::none) const ;
/**
* Testables
*/
/** equals specialized to this factor */
virtual bool equals(const This& expected, double tol=1e-9) const {
return fabs(this->delta_t_ - expected.delta_t_) < tol &&
fabs(this->tau_ - expected.tau_) < tol &&
gtsam::equal_with_abs_tol(this->Qc_, expected.Qc_, tol) &&
gtsam::equal_with_abs_tol(this->Lambda_, expected.Lambda_, tol) &&
gtsam::equal_with_abs_tol(this->Psi_, expected.Psi_, tol);
}
/** print contents */
void print(const std::string& s="") const {
std::cout << s << "GaussianProcessInterpolatorPose3VW" << std::endl;
std::cout << "delta_t = " << delta_t_ << ", tau = " << tau_ << std::endl;
//std::cout << "Qc = " << Qc_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(delta_t_);
ar & BOOST_SERIALIZATION_NVP(tau_);
using namespace boost::serialization;
ar & make_nvp("Qc", make_array(Qc_.data(), Qc_.size()));
ar & make_nvp("Lambda", make_array(Lambda_.data(), Lambda_.size()));
ar & make_nvp("Psi", make_array(Psi_.data(), Psi_.size()));
}
};
} // \ namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessInterpolatorPose3VW> : public Testable<
gpslam::GaussianProcessInterpolatorPose3VW> {};
}
================================================
FILE: gpslam/gp/GaussianProcessInterpolatorRot3.h
================================================
/**
* @file GaussianProcessInterpolatorRot3.h
* @brief Base and utils for Gaussian Process Interpolated measurement factor, SO(3) version
* @author Jing Dong
*/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Matrix.h>
namespace gpslam {
/**
* 4-way factor for Gaussian Process interpolator, SO(3) version
* interpolate pose and velocity given consecutive poses and velocities
*/
class GaussianProcessInterpolatorRot3 {
private:
typedef GaussianProcessInterpolatorRot3 This;
double delta_t_; // t_{i+1} - t_i
double tau_; // tau - t_i. we use tau as time interval from t_i instead of from t_0 as in Barfoot papers
gtsam::Matrix3 Qc_;
gtsam::Matrix6 Lambda_;
gtsam::Matrix6 Psi_;
public:
/// Default constructor: only for serialization
GaussianProcessInterpolatorRot3() {}
/**
* Constructor
* @param Qc noise model of Qc
* @param delta_t the time between the two states
* @param tau the time of interval status
*/
GaussianProcessInterpolatorRot3(const gtsam::SharedNoiseModel& Qc_model, double delta_t, double tau) :
delta_t_(delta_t), tau_(tau) {
// Calcuate Lambda and Psi
Qc_ = getQc(Qc_model);
Lambda_ = calcLambda(Qc_, delta_t_, tau_);
Psi_ = calcPsi(Qc_, delta_t_, tau_);
}
/** Virtual destructor */
virtual ~GaussianProcessInterpolatorRot3() {}
/// interpolate pose with Jacobians
gtsam::Rot3 interpolatePose(const gtsam::Rot3& pose1, const gtsam::Vector3& vel1,
const gtsam::Rot3& pose2, const gtsam::Vector3& vel2,
gtsam::OptionalJacobian<3, 3> H1 = boost::none, gtsam::OptionalJacobian<3, 3> H2 = boost::none,
gtsam::OptionalJacobian<3, 3> H3 = boost::none, gtsam::OptionalJacobian<3, 3> H4 = boost::none) const {
using namespace gtsam;
const Vector6 r1 = (Vector6() << Vector3::Zero(), vel1).finished();
Matrix3 Hinv, Hcomp11, Hcomp12, Hlogmap;
Vector3 r;
if (H1 || H2 || H3 || H4)
r = Rot3::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp11, Hcomp12), Hlogmap);
else
r = Rot3::Logmap(pose1.inverse().compose(pose2));
const Vector6 r2 = (Vector6() << r, vel2).finished();
Rot3 pose;
if (H1 || H2 || H3 || H4) {
Matrix3 Hcomp21, Hcomp22, Hexp;
pose = pose1.compose(Rot3::Expmap(Lambda_.block<3, 6>(0, 0) * r1 + Psi_.block<3, 6>(0, 0) * r2, Hexp), Hcomp21, Hcomp22);
Matrix3 Hexpr1 = Hcomp22*Hexp;
if (H1) *H1 = Hcomp21 + Hexpr1*Psi_.block<3, 3>(0, 0)*Hlogmap*Hcomp11*Hinv;
if (H2) *H2 = Hexpr1*Lambda_.block<3, 3>(0, 3);
if (H3) *H3 = Hexpr1*Psi_.block<3, 3>(0, 0)*Hlogmap*Hcomp12;
if (H4) *H4 = Hexpr1*Psi_.block<3, 3>(0, 3);
} else {
pose = pose1.compose(Rot3::Expmap(Lambda_.block<3, 6>(0, 0) * r1 + Psi_.block<3, 6>(0, 0) * r2));
}
return pose;
}
/// update jacobian based on interpolated jacobians
static void updatePoseJacobians(const gtsam::Matrix& Hpose, const gtsam::Matrix& Hint1,
const gtsam::Matrix& Hint2, const gtsam::Matrix& Hint3, const gtsam::Matrix& Hint4,
boost::optional<gtsam::Matrix&> H1, boost::optional<gtsam::Matrix&> H2,
boost::optional<gtsam::Matrix&> H3, boost::optional<gtsam::Matrix&> H4) {
if (H1) *H1 = Hpose * Hint1;
if (H2) *H2 = Hpose * Hint2;
if (H3) *H3 = Hpose * Hint3;
if (H4) *H4 = Hpose * Hint4;
}
/// interpolate velocity with Jacobians
/// TODO: implementation
gtsam::Vector3 interpolateVelocity(const gtsam::Rot3& pose1, const gtsam::Vector3& vel1,
const gtsam::Rot3& pose2, const gtsam::Vector3& vel2,
gtsam::OptionalJacobian<3, 3> H1 = boost::none, gtsam::OptionalJacobian<3, 3> H2 = boost::none,
gtsam::OptionalJacobian<3, 3> H3 = boost::none, gtsam::OptionalJacobian<3, 3> H4 = boost::none) const ;
/**
* Testables
*/
/** equals specialized to this factor */
virtual bool equals(const This& expected, double tol=1e-9) const {
return fabs(this->delta_t_ - expected.delta_t_) < tol &&
fabs(this->tau_ - expected.tau_) < tol &&
gtsam::equal_with_abs_tol(this->Qc_, expected.Qc_, tol) &&
gtsam::equal_with_abs_tol(this->Lambda_, expected.Lambda_, tol) &&
gtsam::equal_with_abs_tol(this->Psi_, expected.Psi_, tol);
}
/** print contents */
void print(const std::string& s="") const {
std::cout << s << "GaussianProcessInterpolatorRot3" << std::endl;
std::cout << "delta_t = " << delta_t_ << ", tau = " << tau_ << std::endl;
//std::cout << "Qc = " << Qc_ << std::endl;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(delta_t_);
ar & BOOST_SERIALIZATION_NVP(tau_);
using namespace boost::serialization;
ar & make_nvp("Qc", make_array(Qc_.data(), Qc_.size()));
ar & make_nvp("Lambda", make_array(Lambda_.data(), Lambda_.size()));
ar & make_nvp("Psi", make_array(Psi_.data(), Psi_.size()));
}
};
} // \ namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessInterpolatorRot3> : public Testable<
gpslam::GaussianProcessInterpolatorRot3> {};
}
================================================
FILE: gpslam/gp/GaussianProcessPriorLinear.h
================================================
/**
* @file GaussianProcessPriorLinear.h
* @brief Linear GP prior, see Barfoot14rss
* @author Xinyan Yan, Jing Dong
**/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <boost/lexical_cast.hpp>
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
namespace gpslam {
/**
* 4-way factor for Gaussian Process prior factor, linear version
*/
template <int Dim>
class GaussianProcessPriorLinear: public gtsam::NoiseModelFactor4<
Eigen::Matrix<double, Dim, 1>, Eigen::Matrix<double, Dim, 1>,
Eigen::Matrix<double, Dim, 1>, Eigen::Matrix<double, Dim, 1> > {
private:
double delta_t_;
typedef Eigen::Matrix<double, Dim, 1> VectorDim;
typedef Eigen::Matrix<double, 2*Dim, 1> Vector2Dim;
typedef Eigen::Matrix<double, Dim, Dim> MatrixDim;
typedef Eigen::Matrix<double, 2*Dim, Dim> Matrix21Dim;
typedef Eigen::Matrix<double, 2*Dim, 2*Dim> Matrix2Dim;
typedef GaussianProcessPriorLinear<Dim> This;
typedef gtsam::NoiseModelFactor4<VectorDim, VectorDim, VectorDim, VectorDim> Base;
public:
GaussianProcessPriorLinear() {} /* Default constructor only for serialization */
/// Constructor
/// @param delta_t is the time between the two states
GaussianProcessPriorLinear(gtsam::Key poseKey1, gtsam::Key velKey1,
gtsam::Key poseKey2, gtsam::Key velKey2,
double delta_t, const gtsam::SharedNoiseModel& Qc_model) :
Base(gtsam::noiseModel::Gaussian::Covariance(calcQ<Dim>(getQc(Qc_model), delta_t)),
poseKey1, velKey1, poseKey2, velKey2) {
delta_t_ = delta_t;
}
virtual ~GaussianProcessPriorLinear() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// factor error function
gtsam::Vector evaluateError(
const VectorDim& pose1, const VectorDim& vel1,
const VectorDim& pose2, const VectorDim& vel2,
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none,
boost::optional<gtsam::Matrix&> H4 = boost::none) const {
// state vector
Vector2Dim x1 = (Vector2Dim() << pose1, vel1).finished();
Vector2Dim x2 = (Vector2Dim() << pose2, vel2).finished();
// Jacobians
if (H1) *H1 = (Matrix21Dim() << MatrixDim::Identity(), MatrixDim::Zero()).finished();
if (H2) *H2 = (Matrix21Dim() << delta_t_ * MatrixDim::Identity(), MatrixDim::Identity()).finished();
if (H3) *H3 = (Matrix21Dim() << -1.0 * MatrixDim::Identity(), MatrixDim::Zero()).finished();
if (H4) *H4 = (Matrix21Dim() << MatrixDim::Zero(), -1.0 * MatrixDim::Identity()).finished();
// transition matrix & error
return calcPhi<Dim>(delta_t_) * x1 - x2;
}
/** demensions */
size_t dim() const { return Dim; }
/** number of variables attached to this factor */
size_t size() const {
return 4;
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->delta_t_ - e->delta_t_) < tol;
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "4-way Gaussian Process Factor Linear<" << Dim << ">" << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(delta_t_);
}
}; // GaussianProcessPriorLinear
} // namespace gpslam
/// traits
namespace gtsam {
template<int Dim>
struct traits<gpslam::GaussianProcessPriorLinear<Dim> > : public Testable<
gpslam::GaussianProcessPriorLinear<Dim> > {};
}
================================================
FILE: gpslam/gp/GaussianProcessPriorPose2.h
================================================
/**
* @file GaussianProcessPriorPose2.h
* @brief Pose2 GP prior
* @author Jing Dong
**/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <boost/lexical_cast.hpp>
#include <boost/serialization/export.hpp>
#include <ostream>
namespace gpslam {
/**
* 4-way factor for Gaussian Process prior factor, SE(2) version
*/
class GaussianProcessPriorPose2: public gtsam::NoiseModelFactor4<gtsam::Pose2,
gtsam::Vector3, gtsam::Pose2, gtsam::Vector3> {
private:
double delta_t_;
typedef GaussianProcessPriorPose2 This;
typedef gtsam::NoiseModelFactor4<gtsam::Pose2, gtsam::Vector3, gtsam::Pose2, gtsam::Vector3> Base;
public:
GaussianProcessPriorPose2() {} /* Default constructor only for serialization */
/// Constructor
/// @param delta_t is the time between the two states
GaussianProcessPriorPose2(gtsam::Key poseKey1, gtsam::Key velKey1,
gtsam::Key poseKey2, gtsam::Key velKey2,
double delta_t, const gtsam::SharedNoiseModel& Qc_model) :
Base(gtsam::noiseModel::Gaussian::Covariance(calcQ<3>(getQc(Qc_model), delta_t)),
poseKey1, velKey1, poseKey2, velKey2) {
delta_t_ = delta_t;
}
virtual ~GaussianProcessPriorPose2() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// factor error function
gtsam::Vector evaluateError(
const gtsam::Pose2& pose1, const gtsam::Vector3& vel1,
const gtsam::Pose2& pose2, const gtsam::Vector3& vel2,
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none,
boost::optional<gtsam::Matrix&> H4 = boost::none) const {
using namespace gtsam;
Matrix3 Hinv, Hcomp1, Hcomp2, Hlogmap;
Vector3 r;
if (H1 || H2 || H3 || H4)
r = Pose2::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp1, Hcomp2), Hlogmap);
else
r = Pose2::Logmap(pose1.inverse().compose(pose2));
// jacobians
if (H1) *H1 = (Matrix63() << Hlogmap * Hcomp1 * Hinv, Matrix3::Zero()).finished();
if (H2) *H2 = (Matrix63() << -delta_t_ * Matrix3::Identity(), -Matrix3::Identity()).finished();
if (H3) *H3 = (Matrix63() << Hlogmap * Hcomp2, Matrix3::Zero()).finished();
if (H4) *H4 = (Matrix63() << Matrix3::Zero(), Matrix3::Identity()).finished();
return (Vector(6) << (r - vel1 * delta_t_), (vel2 - vel1)).finished();
}
/** number of variables attached to this factor */
size_t size() const {
return 4;
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->delta_t_ - e->delta_t_) < tol;
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "4-way Gaussian Process Factor Pose2" << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(delta_t_);
}
}; // GaussianProcessPriorPose2
} // namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessPriorPose2> : public Testable<gpslam::GaussianProcessPriorPose2> {};
}
================================================
FILE: gpslam/gp/GaussianProcessPriorPose3.h
================================================
/**
* @file GaussianProcessPriorPose3.h
* @brief Pose3 GP prior
* @author Xinyan Yan, Jing Dong
**/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gpslam/gp/Pose3utils.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <boost/lexical_cast.hpp>
#include <boost/serialization/export.hpp>
#include <ostream>
namespace gpslam {
/**
* 4-way factor for Gaussian Process prior factor, SE(3) version
*/
class GaussianProcessPriorPose3: public gtsam::NoiseModelFactor4<gtsam::Pose3,
gtsam::Vector6, gtsam::Pose3, gtsam::Vector6> {
private:
double delta_t_;
typedef GaussianProcessPriorPose3 This;
typedef gtsam::NoiseModelFactor4<gtsam::Pose3, gtsam::Vector6, gtsam::Pose3, gtsam::Vector6> Base;
public:
GaussianProcessPriorPose3() {} /* Default constructor only for serialization */
/// Constructor
/// @param delta_t is the time between the two states
GaussianProcessPriorPose3(gtsam::Key poseKey1, gtsam::Key velKey1,
gtsam::Key poseKey2, gtsam::Key velKey2,
double delta_t, const gtsam::SharedNoiseModel& Qc_model) :
Base(gtsam::noiseModel::Gaussian::Covariance(calcQ<6>(getQc(Qc_model), delta_t)),
poseKey1, velKey1, poseKey2, velKey2) {
delta_t_ = delta_t;
}
virtual ~GaussianProcessPriorPose3() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// factor error function
gtsam::Vector evaluateError(const gtsam::Pose3& pose1, const gtsam::Vector6& vel1,
const gtsam::Pose3& pose2, const gtsam::Vector6& vel2,
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none,
boost::optional<gtsam::Matrix&> H4 = boost::none) const {
using namespace gtsam;
Matrix6 Hinv, Hcomp1, Hcomp2, Hlogmap;
Vector6 r;
if (H1 || H2 || H3 || H4)
r = Pose3::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp1, Hcomp2), Hlogmap);
else
r = Pose3::Logmap(pose1.inverse().compose(pose2));
const Matrix6 Jinv = rightJacobianPose3inv(r);
// jacobians
if (H1) {
const Matrix6 J_Ti = Hlogmap * Hcomp1 * Hinv;
const Matrix6 Jdiff_Ti = jacobianMethodNumercialDiff(rightJacobianPose3inv, r,
vel2) * J_Ti;
*H1 = (Matrix_12_6() << J_Ti, Jdiff_Ti).finished();
}
if (H2) *H2 = (Matrix_12_6() << -delta_t_ * Matrix6::Identity(), -Matrix6::Identity()).finished();
if (H3) {
const Matrix6 J_Ti1 = Hlogmap * Hcomp2;
const Matrix6 Jdiff_Ti1 = jacobianMethodNumercialDiff(rightJacobianPose3inv, r,
vel2) * J_Ti1;
*H3 = (Matrix_12_6() << J_Ti1, Jdiff_Ti1).finished();
}
if (H4) *H4 = (Matrix_12_6() << Matrix6::Zero(), Jinv).finished();
return (Vector(12) << (r - vel1 * delta_t_), (Jinv * vel2 - vel1)).finished();
}
/** number of variables attached to this factor */
size_t size() const {
return 4;
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->delta_t_ - e->delta_t_) < tol;
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "4-way Gaussian Process Factor Pose3" << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(delta_t_);
}
}; // GaussianProcessPriorPose3
} // namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessPriorPose3> : public Testable<gpslam::GaussianProcessPriorPose3> {};
}
================================================
FILE: gpslam/gp/GaussianProcessPriorPose3VW.h
================================================
/**
* @file GaussianProcessPriorPose3VW.h
* @brief Pose3 GP prior, use V/W velocity representation
* @author Xinyan Yan, Jing Dong
**/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gpslam/gp/Pose3utils.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <boost/lexical_cast.hpp>
#include <ostream>
namespace gpslam {
/**
* 4-way factor for Gaussian Process prior factor, SE(3) version
* 6-DOF velocity is represented by 3-DOF translational and 3-DOF rotational velocities (in body frame).
*/
class GaussianProcessPriorPose3VW: public gtsam::NoiseModelFactor6<gtsam::Pose3,
gtsam::Vector3, gtsam::Vector3, gtsam::Pose3, gtsam::Vector3, gtsam::Vector3> {
private:
double delta_t_;
typedef GaussianProcessPriorPose3VW This;
typedef gtsam::NoiseModelFactor6<gtsam::Pose3, gtsam::Vector3, gtsam::Vector3,
gtsam::Pose3, gtsam::Vector3, gtsam::Vector3> Base;
public:
GaussianProcessPriorPose3VW() {} /* Default constructor only for serialization */
/// Constructor
/// @param delta_t is the time between the two states
GaussianProcessPriorPose3VW(
gtsam::Key poseKey1, gtsam::Key velKey1, gtsam::Key omegaKey1,
gtsam::Key poseKey2, gtsam::Key velKey2, gtsam::Key omegaKey2,
double delta_t, const gtsam::SharedNoiseModel& Qc_model) :
Base(gtsam::noiseModel::Gaussian::Covariance(calcQ<6>(getQc(Qc_model), delta_t)),
poseKey1, velKey1, omegaKey1, poseKey2, velKey2, omegaKey2) {
delta_t_ = delta_t;
}
virtual ~GaussianProcessPriorPose3VW() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// factor error function
gtsam::Vector evaluateError(
const gtsam::Pose3& pose1, const gtsam::Vector3& vel1, const gtsam::Vector3& omega1,
const gtsam::Pose3& pose2, const gtsam::Vector3& vel2, const gtsam::Vector3& omega2,
boost::optional<gtsam::Matrix&> H1 = boost::none, boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none, boost::optional<gtsam::Matrix&> H4 = boost::none,
boost::optional<gtsam::Matrix&> H5 = boost::none, boost::optional<gtsam::Matrix&> H6 = boost::none) const {
using namespace gtsam;
Matrix6 Hinv, Hcomp1, Hcomp2, Hlogmap;
Vector6 r;
if (H1 || H4)
r = Pose3::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp1, Hcomp2), Hlogmap);
else
r = Pose3::Logmap(pose1.inverse().compose(pose2));
const Matrix6 Jinv = rightJacobianPose3inv(r);
// convert body frame velocity to body center
Matrix63 H1v, H1w, H2v, H2w;
Matrix6 H1p, H2p;
Matrix_12_6 Hv1, Hv2;
Vector6 v1, v2;
if (H2 || H3 || H5 || H6) {
v1 = convertVWtoVb(vel1, omega1, pose1, H1v, H1w, H1p);
v2 = convertVWtoVb(vel2, omega2, pose2, H2v, H2w, H2p);
Hv1 = (Matrix_12_6() << -delta_t_ * Matrix6::Identity(), -Matrix6::Identity()).finished();
Hv2 = (Matrix_12_6() << Matrix6::Zero(), Jinv).finished();
} else {
v1 = convertVWtoVb(vel1, omega1, pose1);
v2 = convertVWtoVb(vel2, omega2, pose2);
}
// jacobians
if (H1) {
const Matrix6 J_Ti = Hlogmap * Hcomp1 * Hinv;
const Matrix6 Jdiff_Ti = jacobianMethodNumercialDiff(rightJacobianPose3inv, r,
v2) * J_Ti;
*H1 = (Matrix_12_6() << J_Ti-delta_t_*H1p, Jdiff_Ti-H1p).finished();
}
if (H2) *H2 = Hv1 * H1v;
if (H3) *H3 = Hv1 * H1w;
if (H4) {
const Matrix6 J_Ti1 = Hlogmap * Hcomp2;
const Matrix6 Jdiff_Ti1 = jacobianMethodNumercialDiff(rightJacobianPose3inv, r,
v2) * J_Ti1;
*H4 = (Matrix_12_6() << J_Ti1, Jdiff_Ti1 + Jinv*H2p).finished();
}
if (H5) *H5 = Hv2 * H2v;
if (H6) *H6 = Hv2 * H2w;
return (Vector(12) << (r - v1 * delta_t_), Jinv * v2 - v1).finished();
}
/** number of variables attached to this factor */
size_t size() const {
return 6;
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->delta_t_ - e->delta_t_) < tol;
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "4-way Gaussian Process Factor Pose3 VW" << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(delta_t_);
}
}; // GaussianProcessPriorPose3
} // namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessPriorPose3VW> : public Testable<gpslam::GaussianProcessPriorPose3VW> {};
}
================================================
FILE: gpslam/gp/GaussianProcessPriorRot3.h
================================================
/**
* @file GaussianProcessPriorRot3.h
* @brief Rot3 GP prior
* @author Jing Dong
**/
#pragma once
#include <gpslam/gp/GPutils.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <boost/lexical_cast.hpp>
#include <boost/serialization/export.hpp>
#include <ostream>
namespace gpslam {
/**
* 4-way factor for Gaussian Process prior factor, SO(3) version
*/
class GaussianProcessPriorRot3: public gtsam::NoiseModelFactor4<gtsam::Rot3,
gtsam::Vector3, gtsam::Rot3, gtsam::Vector3> {
private:
double delta_t_;
typedef GaussianProcessPriorRot3 This;
typedef gtsam::NoiseModelFactor4<gtsam::Rot3, gtsam::Vector3, gtsam::Rot3, gtsam::Vector3> Base;
public:
GaussianProcessPriorRot3() {} /* Default constructor only for serialization */
/// Constructor
/// @param delta_t is the time between the two states
GaussianProcessPriorRot3(gtsam::Key poseKey1, gtsam::Key velKey1,
gtsam::Key poseKey2, gtsam::Key velKey2,
double delta_t, const gtsam::SharedNoiseModel& Qc_model) :
Base(gtsam::noiseModel::Gaussian::Covariance(calcQ<3>(getQc(Qc_model), delta_t)),
poseKey1, velKey1, poseKey2, velKey2) {
delta_t_ = delta_t;
}
virtual ~GaussianProcessPriorRot3() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// factor error function
gtsam::Vector evaluateError(const gtsam::Rot3& pose1, const gtsam::Vector3& vel1,
const gtsam::Rot3& pose2, const gtsam::Vector3& vel2,
boost::optional<gtsam::Matrix&> H1 = boost::none, boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none, boost::optional<gtsam::Matrix&> H4 = boost::none) const {
using namespace gtsam;
Matrix3 Hinv, Hcomp1, Hcomp2, Hlogmap;
Vector3 r;
if (H1 || H2 || H3 || H4)
r = Rot3::Logmap(pose1.inverse(Hinv).compose(pose2, Hcomp1, Hcomp2), Hlogmap);
else
r = Rot3::Logmap(pose1.inverse().compose(pose2));
// jacobians
if (H1) *H1 = (Matrix63() << Hlogmap * Hcomp1 * Hinv, Matrix3::Zero()).finished();
if (H2) *H2 = (Matrix63() << -delta_t_ * Matrix3::Identity(), -Matrix3::Identity()).finished();
if (H3) *H3 = (Matrix63() << Hlogmap * Hcomp2, Matrix3::Zero()).finished();
if (H4) *H4 = (Matrix63() << Matrix3::Zero(), Matrix3::Identity()).finished();
return (Vector(6) << (r - vel1 * delta_t_), (vel2 - vel1)).finished();
}
/** number of variables attached to this factor */
size_t size() const {
return 4;
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->delta_t_ - e->delta_t_) < tol;
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "4-way Gaussian Process Factor Rot3" << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(delta_t_);
}
}; // GaussianProcessPriorRot3
} // namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GaussianProcessPriorRot3> : public Testable<gpslam::GaussianProcessPriorRot3> {};
}
================================================
FILE: gpslam/gp/Pose3utils.cpp
================================================
/**
* @file Pose3utils.cpp
* @author Jing Dong
**/
#include <gpslam/gp/Pose3utils.h>
#include <cmath>
using namespace gtsam;
namespace gpslam {
/* ************************************************************************** */
// see Barfoot14tro eq. (25)
Vector6 getBodyCentricVb(const Pose3& pose1, const Pose3& pose2, double delta_t) {
return Pose3::Logmap(pose1.inverse().compose(pose2)) / delta_t;
}
/* ************************************************************************** */
Vector6 getBodyCentricVs(const Pose3& pose1, const Pose3& pose2, double delta_t) {
return Pose3::Logmap(pose2.compose(pose1.inverse())) / delta_t;
}
/* ************************************************************************** */
void convertVbtoVW(const Vector6& v6, const Pose3& pose, Vector3& v, Vector3& w,
OptionalJacobian<3, 6> Hv, OptionalJacobian<3, 6> Hw) {
Matrix3 Hrv;
if (Hv) {
v = pose.rotation().rotate(v6.tail<3>(), boost::none, Hrv);
*Hv = (Matrix36() << Matrix3::Zero(), Hrv).finished();
} else {
v = pose.rotation().rotate(v6.tail<3>());
}
if (Hw) {
w = pose.rotation().rotate(v6.head<3>(), boost::none, Hrv);
*Hw = (Matrix36() << Hrv, Matrix3::Zero()).finished();
} else {
w = pose.rotation().rotate(v6.head<3>());
}
}
/* ************************************************************************** */
Vector6 convertVWtoVb(const Vector3& v, const Vector3& w, const Pose3& pose,
OptionalJacobian<6, 3> Hv, OptionalJacobian<6, 3> Hw,
OptionalJacobian<6, 6> Hpose) {
Vector6 v6;
if (Hv || Hw || Hpose) {
Matrix3 Hrv, Hrw, Hpv, Hpw;
v6 = (Vector6() << pose.rotation().unrotate(w, Hpw, Hrw),
pose.rotation().unrotate(v, Hpv, Hrv)).finished();
if (Hv) *Hv = (Matrix63() << Matrix3::Zero(), Hrv).finished();
if (Hw) *Hw = (Matrix63() << Hrw, Matrix3::Zero()).finished();
if (Hpose) *Hpose = (Matrix6() << Hpw, Matrix3::Zero(), Hpv, Matrix3::Zero()).finished();
} else {
v6 = (Vector6() << pose.rotation().unrotate(w),
pose.rotation().unrotate(v)).finished();
}
return v6;
}
/* ************************************************************************** */
// see Barfoot14tro eq. (102)
Matrix3 leftJacobianPose3Q(const Vector6& xi) {
const Vector3 omega = xi.head(3), rho = xi.tail(3);
const double theta = omega.norm(); // rotation angle
const Matrix3 X = skewSymmetric(omega), Y = skewSymmetric(rho);
const Matrix3 XY = X*Y, YX = Y*X, XYX = X * YX;
if (fabs(theta) > 1e-5) {
const double sin_theta = sin(theta), cos_theta = cos(theta);
const double theta2 = theta * theta, theta3 = theta2 * theta,
theta4 = theta3 * theta, theta5 = theta4 * theta;
return 0.5*Y + (theta - sin_theta)/theta3 * (XY + YX + XYX)
- (1.0 - 0.5*theta2 - cos_theta)/theta4 * (X*XY + YX*X - 3.0*XYX)
- 0.5*((1.0 - 0.5*theta2 - cos_theta)/theta4 - 3.0*(theta - sin_theta - theta3/6.0)/theta5)
* (XYX*X + X*XYX);
} else {
return 0.5*Y + 1.0/6.0*(XY + YX + XYX)
- 1.0/24.0*(X*XY + YX*X - 3.0*XYX)
-0.5*(1.0/24.0 + 3.0/120.0) * (XYX*X + X*XYX);
}
}
/* ************************************************************************** */
Matrix3 rightJacobianPose3Q(const Vector6& xi) {
const Vector3 omega = xi.head(3), rho = xi.tail(3);
const double theta = omega.norm(); // rotation angle
const Matrix3 X = skewSymmetric(omega), Y = skewSymmetric(rho);
const Matrix3 XY = X*Y, YX = Y*X, XYX = X * YX;
if (fabs(theta) > 1e-5) {
const double sin_theta = sin(theta), cos_theta = cos(theta);
const double theta2 = theta * theta, theta3 = theta2 * theta,
theta4 = theta3 * theta, theta5 = theta4 * theta;
return -0.5*Y + (theta - sin_theta)/theta3 * (XY + YX - XYX)
+ (1.0 - 0.5*theta2 - cos_theta)/theta4 * (X*XY + YX*X - 3.0*XYX)
- 0.5*((1.0 - 0.5*theta2 - cos_theta)/theta4 - 3.0*(theta - sin_theta - theta3/6.0)/theta5)
* (XYX*X + X*XYX);
} else {
return -0.5*Y + 1.0/6.0*(XY + YX - XYX)
+ 1.0/24.0*(X*XY + YX*X - 3.0*XYX)
-0.5*(1.0/24.0 + 3.0/120.0) * (XYX*X + X*XYX);
}
}
/* ************************************************************************** */
// see Barfoot14tro eq. (100), note that in GTSAM rotation first in pose logmap
Matrix6 leftJacobianPose3(const Vector6& xi) {
const Vector3 omega = xi.head(3);
const Matrix3 Q = leftJacobianPose3Q(xi);
const Matrix3 J = leftJacobianRot3(omega);
return (Matrix6() << J, Matrix::Zero(3,3), Q, J).finished();
}
/* ************************************************************************** */
// see Barfoot14tro eq. (103)
Matrix6 leftJacobianPose3inv(const Vector6& xi) {
const Vector3 omega = xi.head(3);
const Matrix3 Q = leftJacobianPose3Q(xi);
const Matrix3 Jinv = leftJacobianRot3inv(omega);
return (Matrix6() << Jinv, Matrix::Zero(3,3), -Jinv*Q*Jinv, Jinv).finished();
}
/* ************************************************************************** */
// see Barfoot14tro eq. (98)
Matrix3 leftJacobianRot3(const Vector3& omega) {
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return Matrix::Identity(3,3);
const double theta = std::sqrt(theta2); // rotation angle
const Vector3 dir = omega / theta; // direction
const double sin_theta = sin(theta);
const Matrix3 A = skewSymmetric(omega) / theta;
return sin_theta / theta * Matrix::Identity(3,3) + (1 - sin_theta / theta) *
(dir * dir.transpose()) + (1 - cos(theta))/(theta) * A;
}
/* ************************************************************************** */
// see Barfoot14tro eq. (99)
Matrix3 leftJacobianRot3inv(const Vector3& omega) {
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return Matrix::Identity(3,3);
const double theta = std::sqrt(theta2); // rotation angle
const Vector3 dir = omega / theta; // direction
const double theta_2 = theta/2.0;
const double cot_theta_2 = 1.0 / tan(theta_2);
const Matrix3 A = skewSymmetric(omega) / theta;
return theta_2*cot_theta_2 * Matrix::Identity(3,3) + (1 - theta_2*cot_theta_2) *
(dir * dir.transpose()) - theta_2 * A;
}
/* ************************************************************************** */
Matrix6 jacobianMethodNumercialDiff(boost::function<Matrix6(const Vector6&)> func,
const Vector6& xi, const Vector6& x, double dxi) {
Matrix6 Diff = Matrix6();
for (size_t i = 0; i < 6; i++) {
Vector6 xi_dxip = xi, xi_dxin = xi;
xi_dxip(i) += dxi;
Matrix6 Jdiffp = func(xi_dxip);
xi_dxin(i) -= dxi;
Matrix6 Jdiffn = func(xi_dxin);
Diff.block<6,1>(0,i) = (Jdiffp - Jdiffn) / (2.0 * dxi) * x;
}
return Diff;
}
/* ************************************************************************** */
Matrix6 rightJacobianPose3(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Matrix3 Jw = rightJacobianRot3(w);
const Matrix3 Q = rightJacobianPose3Q(xi);
Matrix6 J;
J << Jw, Matrix::Zero(3,3), Q, Jw;
return J;
}
/* ************************************************************************** */
Matrix6 rightJacobianPose3inv(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Matrix3 Jw = rightJacobianRot3inv(w);
const Matrix3 Q = rightJacobianPose3Q(xi);
const Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J;
J << Jw, Matrix::Zero(3,3), Q2, Jw;
return J;
}
/* ************************************************************************** */
Matrix3 rightJacobianRot3(const Vector3& omega) {
using std::cos;
using std::sin;
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return Matrix::Identity(3,3);
double theta = std::sqrt(theta2); // rotation angle
const Matrix3 Y = skewSymmetric(omega) / theta;
return Matrix::Identity(3,3) - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
}
/* ************************************************************************** */
Matrix3 rightJacobianRot3inv(const Vector3& omega) {
using std::cos;
using std::sin;
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return Matrix::Identity(3,3);
double theta = std::sqrt(theta2); // rotation angle
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
return Matrix::Identity(3,3) + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X*X;
}
}
================================================
FILE: gpslam/gp/Pose3utils.h
================================================
/**
* @file Pose3utils.h
* @brief Pose3 GP utils, mainly jacobians of expmap/logmap
* @author Xinyan Yan, Jing Dong
**/
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Pose3.h>
#include <boost/function.hpp>
namespace gpslam {
// fixed size matrix for Dim-6 operation
typedef Eigen::Matrix<double, 12, 1> Vector_12;
typedef Eigen::Matrix<double, 12, 12> Matrix_12;
typedef Eigen::Matrix<double, 6, 12> Matrix_6_12;
typedef Eigen::Matrix<double, 12, 6> Matrix_12_6;
typedef Eigen::Matrix<double, 3, 12> Matrix_3_12;
typedef Eigen::Matrix<double, 12, 3> Matrix_12_3;
/// get body-centric/body-frame velocity from two poses and delta_t
gtsam::Vector6 getBodyCentricVs(const gtsam::Pose3& pose1, const gtsam::Pose3& pose2, double delta_t);
gtsam::Vector6 getBodyCentricVb(const gtsam::Pose3& pose1, const gtsam::Pose3& pose2, double delta_t);
/// convert 6d body-frame velocity into 3+3 world-frame line and angular velocity
/// with optional jacobians
void convertVbtoVW(const gtsam::Vector6& v6, const gtsam::Pose3& pose,
gtsam::Vector3& v, gtsam::Vector3& w,
gtsam::OptionalJacobian<3, 6> Hv = boost::none, gtsam::OptionalJacobian<3, 6> Hw = boost::none);
gtsam::Vector6 convertVWtoVb(const gtsam::Vector3& v, const gtsam::Vector3& w,
const gtsam::Pose3& pose, gtsam::OptionalJacobian<6, 3> Hv = boost::none,
gtsam::OptionalJacobian<6, 3> Hw = boost::none,
gtsam::OptionalJacobian<6, 6> Hpose = boost::none);
/// left Jacobian for Pose3 Expmap
gtsam::Matrix6 leftJacobianPose3(const gtsam::Vector6& xi);
gtsam::Matrix6 leftJacobianPose3inv(const gtsam::Vector6& xi);
gtsam::Matrix3 leftJacobianPose3Q(const gtsam::Vector6& xi);
/// right Jacobian for Pose3: jacobian of expmap/logmap
gtsam::Matrix6 rightJacobianPose3(const gtsam::Vector6& xi);
gtsam::Matrix6 rightJacobianPose3inv(const gtsam::Vector6& xi);
gtsam::Matrix3 rightJacobianPose3Q(const gtsam::Vector6& xi);
/// numerical diff of jacobian matrix methods
/// output d(A(xi)*x)/dxi jacobian matrix
gtsam::Matrix6 jacobianMethodNumercialDiff(boost::function<gtsam::Matrix6(
const gtsam::Vector6&)> func, const gtsam::Vector6& xi,
const gtsam::Vector6& x, double dxi = 1e-6);
/// left Jacobian for Rot3 Expmap
gtsam::Matrix3 leftJacobianRot3(const gtsam::Vector3& omega);
gtsam::Matrix3 leftJacobianRot3inv(const gtsam::Vector3& omega);
/// right Jacobian for Rot3: jacobian of expmap/logmap
gtsam::Matrix3 rightJacobianRot3(const gtsam::Vector3& omega);
gtsam::Matrix3 rightJacobianRot3inv(const gtsam::Vector3& omega);
} // namespace gtsam
================================================
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorLinear.cpp
================================================
/**
* @file testGaussianProcessInterpolatorLinear.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gpslam/gp/GaussianProcessInterpolatorLinear.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
// only test the 3 dim linear case (for Pose2 approximation)
typedef GaussianProcessInterpolatorLinear<3> GPBase3;
/* ************************************************************************** */
TEST(GaussianProcessInterpolatorLinear, equals) {
Matrix3 Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
double dt = 0.1, tau = 0.03;
GPBase3 base1(Qc_model, dt, tau), base2(Qc_model, dt, tau);
EXPECT(assert_equal(base1, base2, 1e-9));
Matrix3 Qc2 = 0.02 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc2_model = noiseModel::Gaussian::Covariance(Qc2);
GPBase3 base3(Qc2_model, dt, tau);
EXPECT(!assert_equal(base1, base3, 1e-9));
dt = 0.2, tau = 0.03;
GPBase3 base4(Qc_model, dt, tau);
EXPECT(!assert_equal(base1, base4, 1e-9));
dt = 0.1, tau = 0.06;
GPBase3 base5(Qc_model, dt, tau);
EXPECT(!assert_equal(base1, base5, 1e-9));
}
/* ************************************************************************** */
TEST(GaussianProcessInterpolatorLinear, interpolatePose) {
Vector3 p1, p2, expect, actual;
Vector3 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Matrix3 Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
double dt = 0.1, tau = 0.03;
GPBase3 base(Qc_model, dt, tau);
// test at origin
p1 = (Vector3() << 0, 0, 0).finished();
p2 = (Vector3() << 0, 0, 0).finished();
v1 = (Vector3() << 0, 0, 0).finished();
v2 = (Vector3() << 0, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = (Vector3() << 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test forward velocity
p1 = (Vector3() << 0, 0, 0).finished();
p2 = (Vector3() << 1, 0, 0).finished();
v1 = (Vector3() << 10, 0, 0).finished();
v2 = (Vector3() << 10, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = (Vector3() << 0.3, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test rotate
p1 = (Vector3() << 0, 0, 0).finished();
p2 = (Vector3() << 0, 0, 0.3).finished();
v1 = (Vector3() << 0, 0, 3).finished();
v2 = (Vector3() << 0, 0, 3).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = (Vector3() << 0, 0, 0.09).finished();
expectH1 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// some random stuff just for testing jacobian (error is not zero)
p1 = (Vector3() << 2, -5, 7).finished();
p2 = (Vector3() << -8, 4, -8).finished();
v1 = (Vector3() << -1, 2, -9).finished();
v2 = (Vector3() << 3, -4, 7).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector3(const Vector3&)>(
boost::bind(&GPBase3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorPose2.cpp
================================================
/**
* @file testGaussianProcessInterpolatorPose2.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gpslam/gp/GaussianProcessInterpolatorPose2.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
/* ************************************************************************** */
TEST(GaussianProcessInterpolatorPose2, interpolatePose) {
Pose2 p1, p2, expect, actual;
Vector3 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Matrix3 Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
double dt = 0.1, tau = 0.03;
GaussianProcessInterpolatorPose2 base(Qc_model, dt, tau);
// test at origin
p1 = Pose2(0, 0, 0);
p2 = Pose2(0, 0, 0);
v1 = (Vector3() << 0, 0, 0).finished();
v2 = (Vector3() << 0, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Pose2(0, 0, 0);
expectH1 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
// test forward
p1 = Pose2(0, 0, 0);
p2 = Pose2(0.1, 0, 0);
v1 = (Vector3() << 1, 0, 0).finished();
v2 = (Vector3() << 1, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Pose2(0.03, 0, 0);
expectH1 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-4);
expectH2 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-4);
expectH3 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-4);
expectH4 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-4);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test rotate
p1 = Pose2(0, 0, 0);
p2 = Pose2(0, 0, 0.1);
v1 = (Vector3() << 0, 0, 1).finished();
v2 = (Vector3() << 0, 0, 1).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Pose2(0, 0, 0.03);
expectH1 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
// some random stuff, just test jacobians
p1 = Pose2(3, -8, 2);
p2 = Pose2(-9, 3, 4);
v1 = (Vector3() << 0.5, 0.9, 0.7).finished();
v2 = (Vector3() << 0.6, -0.2, 0.8).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose2(const Pose2&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose2(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorPose2::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorPose3.cpp
================================================
/**
* @file testGaussianProcessInterpolatorPose3.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gpslam/gp/GaussianProcessInterpolatorPose3.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
/* ************************************************************************** */
TEST(GaussianProcessInterpolatorPose3, interpolatePose) {
Pose3 p1, p2, expect, actual;
Vector6 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Matrix6 Qc = 0.01 * Matrix::Identity(6,6);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
double dt = 0.1, tau = 0.03;
GaussianProcessInterpolatorPose3 base(Qc_model, dt, tau);
// test at origin
p1 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
p2 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
v1 = (Vector6() << 0, 0, 0, 0, 0, 0).finished();
v2 = (Vector6() << 0, 0, 0, 0, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
// test forward
p1 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
p2 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0.1, 0, 0));
v1 = (Vector6() << 0, 0, 0, 1, 0, 0).finished();
v2 = (Vector6() << 0, 0, 0, 1, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Pose3(Rot3::Ypr(0, 0, 0), Point3(0.03, 0, 0));
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-4);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-4);
expectH3 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-4);
expectH4 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-4);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test rotate
p1 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
p2 = Pose3(Rot3::Ypr(0.1, 0, 0), Point3(0, 0, 0));
v1 = (Vector6() << 0, 0, 1, 0, 0, 0).finished();
v2 = (Vector6() << 0, 0, 1, 0, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Pose3(Rot3::Ypr(0.03, 0, 0), Point3(0.0, 0, 0));
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
// some random stuff, just test jacobians
p1 = Pose3(Rot3::Ypr(0.4, -0.8, 0.2), Point3(3, -8, 2));
p2 = Pose3(Rot3::Ypr(0.1, 0.3, -0.5), Point3(-9, 3, 4));
v1 = (Vector6() << 0.1, -0.2, -1.4, 0.5, 0.9, 0.7).finished();
v2 = (Vector6() << 0.6, 0.3, -0.9, 0.4, -0.2, 0.8).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose3(const Vector6&)>(
boost::bind(&GaussianProcessInterpolatorPose3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorPose3VW.cpp
================================================
/**
* @file testGaussianProcessInterpolatorPose3VW.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gpslam/gp/GaussianProcessInterpolatorPose3VW.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
Pose3 interWrapper(const GaussianProcessInterpolatorPose3VW& interpolater,
const Pose3& pose1, const Vector3& v1, const Vector3& omega1,
const Pose3& pose2, const Vector3& v2, const Vector3& omega2) {
return interpolater.interpolatePose(pose1, v1, omega1, pose2, v2, omega2);
}
/* ************************************************************************** */
TEST(GaussianProcessInterpolatorPose3VW, interpolatePose) {
Pose3 p1, p2, expect, actual;
Vector3 v1, v2, w1, w2;
Matrix actualH1, actualH2, actualH3, actualH4, actualH5, actualH6;
Matrix expectH1, expectH2, expectH3, expectH4, expectH5, expectH6;
Matrix6 Qc = 0.01 * Matrix::Identity(6,6);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
double dt = 0.1, tau = 0.03;
GaussianProcessInterpolatorPose3VW base(Qc_model, dt, tau);
// test at origin
p1 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
p2 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
v1 = Vector3(0, 0, 0); w1 = Vector3(0, 0, 0);
v2 = Vector3(0, 0, 0); w2 = Vector3(0, 0, 0);
actual = base.interpolatePose(p1, v1, w1, p2, v2, w2, actualH1, actualH2,
actualH3, actualH4, actualH5, actualH6);
expect = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, _1, v1, w1, p2, v2, w2)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, _1, w1, p2, v2, w2)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, _1, p2, v2, w2)), w1, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, p1, v1, w1, _1, v2, w2)), p2, 1e-6);
expectH5 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, _1, w2)), v2, 1e-6);
expectH6 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, v2, _1)), w2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
EXPECT(assert_equal(expectH5, actualH5, 1e-8));
EXPECT(assert_equal(expectH6, actualH6, 1e-8));
// test forward
p1 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
p2 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0.1, 0.2, 0));
v1 = Vector3(1, 2, 0); w1 = Vector3(0, 0, 0);
v2 = Vector3(1, 2, 0); w2 = Vector3(0, 0, 0);
actual = base.interpolatePose(p1, v1, w1, p2, v2, w2, actualH1, actualH2,
actualH3, actualH4, actualH5, actualH6);
expect = Pose3(Rot3::Ypr(0, 0, 0), Point3(0.03, 0.06, 0));
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, _1, v1, w1, p2, v2, w2)), p1, 1e-4);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, _1, w1, p2, v2, w2)), v1, 1e-4);
expectH3 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, _1, p2, v2, w2)), w1, 1e-4);
expectH4 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, p1, v1, w1, _1, v2, w2)), p2, 1e-4);
expectH5 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, _1, w2)), v2, 1e-4);
expectH6 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, v2, _1)), w2, 1e-4);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
EXPECT(assert_equal(expectH5, actualH5, 1e-6));
EXPECT(assert_equal(expectH6, actualH6, 1e-6));
// test rotate
p1 = Pose3(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0));
p2 = Pose3(Rot3::Ypr(0.1, 0, 0), Point3(0, 0, 0));
v1 = Vector3(0, 0, 0); w1 = Vector3(0, 0, 1);
v2 = Vector3(0, 0, 0); w2 = Vector3(0, 0, 1);
actual = base.interpolatePose(p1, v1, w1, p2, v2, w2, actualH1, actualH2,
actualH3, actualH4, actualH5, actualH6);
expect = Pose3(Rot3::Ypr(0.03, 0, 0), Point3(0.0, 0, 0));
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, _1, v1, w1, p2, v2, w2)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, _1, w1, p2, v2, w2)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, _1, p2, v2, w2)), w1, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, p1, v1, w1, _1, v2, w2)), p2, 1e-6);
expectH5 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, _1, w2)), v2, 1e-6);
expectH6 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, v2, _1)), w2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
EXPECT(assert_equal(expectH5, actualH5, 1e-8));
EXPECT(assert_equal(expectH6, actualH6, 1e-8));
// some random stuff, just test jacobians
p1 = Pose3(Rot3::Ypr(0.4, -0.8, 0.2), Point3(3, -8, 2));
p2 = Pose3(Rot3::Ypr(0.1, 0.3, -0.5), Point3(-9, 3, 4));
v1 = Vector3(0.1, -0.2, -1.4); w1 = Vector3(0.5, 0.9, 0.7);
v1 = Vector3(0.6, 0.3, -0.9); w1 = Vector3(0.4, -0.2, 0.8);
actual = base.interpolatePose(p1, v1, w1, p2, v2, w2, actualH1, actualH2,
actualH3, actualH4, actualH5, actualH6);
expectH1 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, _1, v1, w1, p2, v2, w2)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, _1, w1, p2, v2, w2)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, _1, p2, v2, w2)), w1, 1e-6);
expectH4 = numericalDerivative11(boost::function<Pose3(const Pose3&)>(
boost::bind(interWrapper, base, p1, v1, w1, _1, v2, w2)), p2, 1e-6);
expectH5 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, _1, w2)), v2, 1e-6);
expectH6 = numericalDerivative11(boost::function<Pose3(const Vector3&)>(
boost::bind(interWrapper, base, p1, v1, w1, p2, v2, _1)), w2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
EXPECT(assert_equal(expectH5, actualH5, 1e-8));
EXPECT(assert_equal(expectH6, actualH6, 1e-8));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorRot3.cpp
================================================
/**
* @file testGaussianProcessInterpolatorRot3.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gpslam/gp/GaussianProcessInterpolatorRot3.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
/* ************************************************************************** */
TEST(GaussianProcessInterpolatorRot3, interpolatePose) {
Rot3 p1, p2, expect, actual;
Vector3 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Matrix3 Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
double dt = 0.1, tau = 0.03;
GaussianProcessInterpolatorRot3 base(Qc_model, dt, tau);
// test at origin
p1 = Rot3::Ypr(0, 0, 0);
p2 = Rot3::Ypr(0, 0, 0);
v1 = (Vector3() << 0, 0, 0).finished();
v2 = (Vector3() << 0, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Rot3::Ypr(0, 0, 0);
expectH1 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
// test rotate x
p1 = Rot3::Ypr(0, 0, 0);
p2 = Rot3::Ypr(0, 0, 0.1);
v1 = (Vector3() << 1, 0, 0).finished();
v2 = (Vector3() << 1, 0, 0).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Rot3::Ypr(0, 0, 0.03);
expectH1 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test rotate z
p1 = Rot3::Ypr(0, 0, 0);
p2 = Rot3::Ypr(0.1, 0, 0);
v1 = (Vector3() << 0, 0, 1).finished();
v2 = (Vector3() << 0, 0, 1).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expect = Rot3::Ypr(0.03, 0, 0);
expectH1 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
// some random stuff, just test jacobians
p1 = Rot3::Ypr(0.4, -0.8, 0.2);
p2 = Rot3::Ypr(0.1, 0.3, -0.5);
v1 = (Vector3() << 0.1, -0.2, -1.4).finished();
v2 = (Vector3() << 0.6, 0.3, -0.9).finished();
actual = base.interpolatePose(p1, v1, p2, v2, actualH1, actualH2,
actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Rot3(const Rot3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Rot3(const Vector3&)>(
boost::bind(&GaussianProcessInterpolatorRot3::interpolatePose, base,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-8));
EXPECT(assert_equal(expectH2, actualH2, 1e-8));
EXPECT(assert_equal(expectH3, actualH3, 1e-8));
EXPECT(assert_equal(expectH4, actualH4, 1e-8));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessPriorLinear.cpp
================================================
/**
* @file testGaussianProcessPriorLinear.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gpslam/gp/GaussianProcessPriorLinear.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
// only test the 3 dim linear case (for Pose2 approximation)
typedef GaussianProcessPriorLinear<3> GPPrior3;
/* ************************************************************************** */
TEST(GaussianProcessPriorLinear, Factor) {
const double delta_t = 0.1;
Matrix Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Key key_pose1 = Symbol('x', 1), key_pose2 = Symbol('x', 2);
Key key_vel1 = Symbol('v', 1), key_vel2 = Symbol('v', 2);
GPPrior3 factor(key_pose1, key_vel1, key_pose2, key_vel2, delta_t, Qc_model);
Vector3 p1, p2;
Vector3 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Vector actual, expect;
// test at origin
p1 = (Vector3() << 0, 0, 0).finished();
p2 = (Vector3() << 0, 0, 0).finished();
v1 = (Vector3() << 0, 0, 0).finished();
v2 = (Vector3() << 0, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test const forward v1 = v2 = 1.0
p1 = (Vector3() << 0, 0, 0).finished();
p2 = (Vector3() << 0.1, 0, 0).finished();
v1 = (Vector3() << 1, 0, 0).finished();
v2 = (Vector3() << 1, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test const rotation w = 1.0
p1 = (Vector3() << 0, 0, 0).finished();
p2 = (Vector3() << 0, 0, 0.1).finished();
v1 = (Vector3() << 0, 0, 1).finished();
v2 = (Vector3() << 0, 0, 1).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// some random stuff just for testing jacobian (error is not zero)
p1 = (Vector3() << 2, -5, 7).finished();
p2 = (Vector3() << -8, 4, -8).finished();
v1 = (Vector3() << -1, 2, -9).finished();
v2 = (Vector3() << 3, -4, 7).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GPPrior3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
}
/* ************************************************************************** */
TEST(GaussianProcessPriorLinear, Optimization) {
/**
* A simple graph:
*
* p1 p2
* | |
* x1 x2
* \ /
* gp
* / \
* v1 v2
*
* p1 and p2 are pose prior factor to fix the poses, gp is the GP factor
* that get correct velocity of v1 and v2
*/
noiseModel::Isotropic::shared_ptr model_prior =
noiseModel::Isotropic::Sigma(3, 0.001);
double delta_t = 0.1;
Matrix Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Vector3 p1 = (Vector3() << 1, 0, 0).finished();
Vector3 p2 = (Vector3() << 1.1, 0, 0).finished();
Vector3 v1 = (Vector3() << 1, 0, 0).finished();
Vector3 v2 = (Vector3() << 1, 0, 0).finished();
// noisy init values
Vector3 p1init = (Vector3() << 1.1, 0.3, 0.5).finished();
Vector3 p2init = (Vector3() << 1.2, 0.4, -0.2).finished();
Vector3 v1init = (Vector3() << 1, 0.1, 0.2).finished();
Vector3 v2init = (Vector3() << 2.1, -1.2, 0.9).finished();
NonlinearFactorGraph graph;
graph.add(PriorFactor<Vector3>(Symbol('x', 1), p1, model_prior));
graph.add(PriorFactor<Vector3>(Symbol('x', 2), p2, model_prior));
//graph.add(PriorFactor<Vector3>(Symbol('v', 1), v1, model_prior));
graph.add(GPPrior3(Symbol('x', 1), Symbol('v', 1),
Symbol('x', 2), Symbol('v', 2), delta_t, Qc_model));
Values init_values;
init_values.insert(Symbol('x', 1), p1init);
init_values.insert(Symbol('v', 1), v1init);
init_values.insert(Symbol('x', 2), p2init);
init_values.insert(Symbol('v', 2), v2init);
GaussNewtonParams parameters;
GaussNewtonOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values values = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-6);
EXPECT(assert_equal(p1, values.at<Vector3>(Symbol('x', 1)), 1e-6));
EXPECT(assert_equal(p2, values.at<Vector3>(Symbol('x', 2)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector3>(Symbol('v', 1)), 1e-6));
EXPECT(assert_equal(v2, values.at<Vector3>(Symbol('v', 2)), 1e-6));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessPriorPose2.cpp
================================================
/**
* @file testGaussianProcessPriorPose2.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gpslam/gp/GaussianProcessPriorPose2.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
/* ************************************************************************** */
TEST(GaussianProcessPriorPose2, Factor) {
const double delta_t = 0.1;
Matrix Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Key key_pose1 = Symbol('x', 1), key_pose2 = Symbol('x', 2);
Key key_vel1 = Symbol('v', 1), key_vel2 = Symbol('v', 2);
GaussianProcessPriorPose2 factor(key_pose1, key_vel1, key_pose2, key_vel2, delta_t, Qc_model);
Pose2 p1, p2;
Vector3 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Vector actual, expect;
// test at origin
p1 = Pose2(0, 0, 0);
p2 = Pose2(0, 0, 0);
v1 = (Vector3() << 0, 0, 0).finished();
v2 = (Vector3() << 0, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test at const forward velocity v1 = v2 = 1.0;
p1 = Pose2(0, 0, 0);
p2 = Pose2(0.1, 0, 0);
v1 = (Vector3() << 1, 0, 0).finished();
v2 = (Vector3() << 1, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-4);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-4);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-4);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-4);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test at const rotation w1 = w2 = 1.0;
p1 = Pose2(0, 0, 0);
p2 = Pose2(0, 0, 0.1);
v1 = (Vector3() << 0, 0, 1).finished();
v2 = (Vector3() << 0, 0, 1).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// some random stuff just for testing jacobian (error is not zero)
p1 = Pose2(-0.1, 1.2, 0.3);
p2 = Pose2(2.4, -2.5, 3.7);
v1 = (Vector3() << 5, 4, 9).finished();
v2 = (Vector3() << 0, 6, 4).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose2&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorPose2::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
}
/* ************************************************************************** */
TEST(GaussianProcessPriorPose2, Optimization) {
/**
* A simple graph:
*
* p1 p2
* | |
* x1 x2
* \ /
* gp
* / \
* v1 v2
*
* p1 and p2 are pose prior factor to fix the poses, gp is the GP factor
* that get correct velocity of v2
*/
noiseModel::Isotropic::shared_ptr model_prior = noiseModel::Isotropic::Sigma(3, 0.001);
double delta_t = 1;
Matrix Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Pose2 pose1(0,0,0), pose2(1,0,0);
Vector3 v1 = (Vector3() << 1, 0, 0).finished();
Vector3 v2 = (Vector3() << 2.0, -0.5, 0.6).finished(); // rnd value
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose2>(Symbol('x', 1), pose1, model_prior));
graph.add(PriorFactor<Pose2>(Symbol('x', 2), pose2, model_prior));
//graph.add(PriorFactor<Vector6>(Symbol('v', 1), v1, model_prior));
graph.add(GaussianProcessPriorPose2(Symbol('x', 1), Symbol('v', 1),
Symbol('x', 2), Symbol('v', 2), delta_t, Qc_model));
Values init_values;
init_values.insert(Symbol('x', 1), pose1);
init_values.insert(Symbol('v', 1), v1);
init_values.insert(Symbol('x', 2), pose2);
init_values.insert(Symbol('v', 2), v2);
GaussNewtonParams parameters;
GaussNewtonOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values values = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-6);
EXPECT(assert_equal(pose1, values.at<Pose2>(Symbol('x', 1)), 1e-6));
EXPECT(assert_equal(pose2, values.at<Pose2>(Symbol('x', 2)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector3>(Symbol('v', 1)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector3>(Symbol('v', 2)), 1e-6));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessPriorPose3.cpp
================================================
/**
* @file testGaussianProcessPriorPose3.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gpslam/gp/GaussianProcessPriorPose3.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
/* ************************************************************************** */
TEST(GaussianProcessPriorPose3Test, Factor) {
const double delta_t = 0.1;
Matrix Qc = 0.01 * Matrix::Identity(6,6);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Key key_pose1 = Symbol('x', 1), key_pose2 = Symbol('x', 2);
Key key_vel1 = Symbol('v', 1), key_vel2 = Symbol('v', 2);
GaussianProcessPriorPose3 factor(key_pose1, key_vel1, key_pose2, key_vel2, delta_t, Qc_model);
Pose3 p1, p2;
Vector6 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Vector actual, expect;
// test at origin
p1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
p2 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
v1 = (Vector6() << 0, 0, 0, 0, 0, 0).finished();
v2 = (Vector6() << 0, 0, 0, 0, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(12) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test at const forward velocity v1 = v2 = 1.0;
p1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
p2 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.1, 0.0, 0.0));
v1 = (Vector6() << 0, 0, 0, 1, 0, 0).finished();
v2 = (Vector6() << 0, 0, 0, 1, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(12) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test at const rotation w1 = w2 = 1.0;
p1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
p2 = Pose3(Rot3::Ypr(0.1, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
v1 = (Vector6() << 0, 0, 1, 0, 0, 0).finished();
v2 = (Vector6() << 0, 0, 1, 0, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(12) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// some random stuff just for testing jacobian (error is not zero)
p1 = Pose3(Rot3::Ypr(-0.1, 1.2, 0.3), Point3(-4.0, 2.0, 14.0));
p2 = Pose3(Rot3::Ypr(2.4, -2.5, 3.7), Point3(9.0, -8.0, -7.0));
v1 = (Vector6() << 2, 3, 1, 5, 4, 9).finished();
v2 = (Vector6() << 1, 3, 8, 0, 6, 4).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector6&)>(
boost::bind(&GaussianProcessPriorPose3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-5));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-5));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
}
/* ************************************************************************** */
TEST(GaussianProcessPriorPose3Test, Optimization) {
/**
* A simple graph:
*
* p1 p2
* | |
* x1 x2
* \ /
* gp
* / \
* v1 v2
*
* p1 and p2 are pose prior factor to fix the poses, gp is the GP factor
* that get correct velocity of v2
*/
noiseModel::Isotropic::shared_ptr model_prior =
noiseModel::Isotropic::Sigma(6, 0.001);
double delta_t = 1;
Matrix Qc = 0.01 * Matrix::Identity(6,6);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Pose3 pose1(Rot3(), Point3(0,0,0)), pose2(Rot3(), Point3(1,0,0));
Vector6 v1 = (Vector6() << 0, 0, 0, 1, 0, 0).finished();
Vector6 v2 = (Vector6() << 0.1, 0.2, -0.3, 2.0, -0.5, 0.6).finished(); // rnd value
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose3>(Symbol('x', 1), pose1, model_prior));
graph.add(PriorFactor<Pose3>(Symbol('x', 2), pose2, model_prior));
//graph.add(PriorFactor<Vector6>(Symbol('v', 1), v1, model_prior));
graph.add(GaussianProcessPriorPose3(Symbol('x', 1), Symbol('v', 1),
Symbol('x', 2), Symbol('v', 2), delta_t, Qc_model));
Values init_values;
init_values.insert(Symbol('x', 1), pose1);
init_values.insert(Symbol('v', 1), v1);
init_values.insert(Symbol('x', 2), pose2);
init_values.insert(Symbol('v', 2), v2);
GaussNewtonParams parameters;
GaussNewtonOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values values = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-6);
EXPECT(assert_equal(pose1, values.at<Pose3>(Symbol('x', 1)), 1e-6));
EXPECT(assert_equal(pose2, values.at<Pose3>(Symbol('x', 2)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector6>(Symbol('v', 1)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector6>(Symbol('v', 2)), 1e-6));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessPriorPose3VW.cpp
================================================
/**
* @file testGaussianProcessPriorPose3VW.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gpslam/gp/GaussianProcessPriorPose3VW.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
Vector errorWrapper(const GaussianProcessPriorPose3VW& factor,
const Pose3& pose1, const Vector3& vel1, const Vector3& omega1,
const Pose3& pose2, const Vector3& vel2, const Vector3& omega2) {
return factor.evaluateError(pose1, vel1, omega1, pose2, vel2, omega2);
}
/* ************************************************************************** */
TEST(GaussianProcessPriorPose3VW, Factor) {
const double delta_t = 0.1;
Matrix Qc = 0.01 * Matrix::Identity(6,6);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Key key_pose1 = Symbol('x', 1), key_pose2 = Symbol('x', 2);
Key key_vel1 = Symbol('v', 1), key_vel2 = Symbol('v', 2);
Key key_omega1 = Symbol('w', 1), key_omega2 = Symbol('w', 2);
GaussianProcessPriorPose3VW factor(key_pose1, key_vel1, key_omega1, key_pose2, key_vel2, key_omega2, delta_t, Qc_model);
Pose3 p1, p2;
Vector3 v1, v2, w1, w2;
Matrix actualH1, actualH2, actualH3, actualH4, actualH5, actualH6;
Matrix expectH1, expectH2, expectH3, expectH4, expectH5, expectH6;
Vector actual, expect;
// test at origin
p1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
p2 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
v1 = Vector3(0, 0, 0); w1 = Vector3(0, 0, 0);
v2 = Vector3(0, 0, 0); w2 = Vector3(0, 0, 0);
actual = factor.evaluateError(p1, v1, w1, p2, v2, w2, actualH1, actualH2, actualH3, actualH4, actualH5, actualH6);
expect = (Vector(12) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, _1, v1, w1, p2, v2, w2)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, _1, w1, p2, v2, w2)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, _1, p2, v2, w2)), w1, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, _1, v2, w2)), p2, 1e-6);
expectH5 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, _1, w2)), v2, 1e-6);
expectH6 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, v2, _1)), w2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
EXPECT(assert_equal(expectH5, actualH5, 1e-6));
EXPECT(assert_equal(expectH6, actualH6, 1e-6));
// test at const forward velocity v1 = v2 = 1.0;
p1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
p2 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.1, 0.0, 0.0));
v1 = Vector3(1, 0, 0); w1 = Vector3(0, 0, 0);
v2 = Vector3(1, 0, 0); w2 = Vector3(0, 0, 0);
actual = factor.evaluateError(p1, v1, w1, p2, v2, w2, actualH1, actualH2, actualH3, actualH4, actualH5, actualH6);
expect = (Vector(12) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, _1, v1, w1, p2, v2, w2)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, _1, w1, p2, v2, w2)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, _1, p2, v2, w2)), w1, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, _1, v2, w2)), p2, 1e-6);
expectH5 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, _1, w2)), v2, 1e-6);
expectH6 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, v2, _1)), w2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
EXPECT(assert_equal(expectH5, actualH5, 1e-6));
EXPECT(assert_equal(expectH6, actualH6, 1e-6));
// test at const rotation w1 = w2 = 1.0;
p1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
p2 = Pose3(Rot3::Ypr(0.1, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
v1 = Vector3(0, 0, 0); w1 = Vector3(0, 0, 1);
v2 = Vector3(0, 0, 0); w2 = Vector3(0, 0, 1);
actual = factor.evaluateError(p1, v1, w1, p2, v2, w2, actualH1, actualH2, actualH3, actualH4, actualH5, actualH6);
expect = (Vector(12) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, _1, v1, w1, p2, v2, w2)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, _1, w1, p2, v2, w2)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, _1, p2, v2, w2)), w1, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, _1, v2, w2)), p2, 1e-6);
expectH5 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, _1, w2)), v2, 1e-6);
expectH6 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, v2, _1)), w2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
EXPECT(assert_equal(expectH5, actualH5, 1e-6));
EXPECT(assert_equal(expectH6, actualH6, 1e-6));
// some random stuff just for testing jacobian (error is not zero)
p1 = Pose3(Rot3::Ypr(-0.1, 1.2, 0.3), Point3(-4.0, 2.0, 14.0));
p2 = Pose3(Rot3::Ypr(2.4, -2.5, 3.7), Point3(9.0, -8.0, -7.0));
v1 = Vector3(2, 3, 1); w1 = Vector3(5, 4, 9);
v2 = Vector3(1, 3, 8); w1 = Vector3(0, 6, 4);
actual = factor.evaluateError(p1, v1, w1, p2, v2, w2, actualH1, actualH2, actualH3, actualH4, actualH5, actualH6);
expect = (Vector(12) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, _1, v1, w1, p2, v2, w2)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, _1, w1, p2, v2, w2)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, _1, p2, v2, w2)), w1, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Pose3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, _1, v2, w2)), p2, 1e-6);
expectH5 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, _1, w2)), v2, 1e-6);
expectH6 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&errorWrapper, factor, p1, v1, w1, p2, v2, _1)), w2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
EXPECT(assert_equal(expectH5, actualH5, 1e-6));
EXPECT(assert_equal(expectH6, actualH6, 1e-6));
}
/* ************************************************************************** */
TEST(GaussianProcessPriorPose3VW, Optimization) {
/**
* A simple graph:
*
* p1 p2
* | |
* x1 x2
* \ /
* gp
* / \
* v1 v2
*
* p1 and p2 are pose prior factor to fix the poses, gp is the GP factor
* that get correct velocity of v2
*/
noiseModel::Isotropic::shared_ptr model_prior =
noiseModel::Isotropic::Sigma(6, 0.001);
double delta_t = 1;
Matrix Qc = 0.01 * Matrix::Identity(6,6);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Pose3 pose1(Rot3(), Point3(0,0,0)), pose2(Rot3(), Point3(1,0,0));
Vector3 v1 = Vector3(1, 0, 0), w1 = Vector3(0, 0, 0);
Vector3 v2 = Vector3(1, 0, 0), w2 = Vector3(0, 0, 0);
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose3>(Symbol('x', 1), pose1, model_prior));
graph.add(PriorFactor<Pose3>(Symbol('x', 2), pose2, model_prior));
graph.add(GaussianProcessPriorPose3VW(Symbol('x', 1), Symbol('v', 1), Symbol('w', 1),
Symbol('x', 2), Symbol('v', 2), Symbol('w', 2), delta_t, Qc_model));
Values init_values;
init_values.insert(Symbol('x', 1), pose1);
init_values.insert(Symbol('v', 1), v1);
init_values.insert(Symbol('w', 1), w1);
init_values.insert(Symbol('x', 2), pose2);
init_values.insert(Symbol('v', 2), v2);
init_values.insert(Symbol('w', 2), w2);
GaussNewtonParams parameters;
GaussNewtonOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values values = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-6);
EXPECT(assert_equal(pose1, values.at<Pose3>(Symbol('x', 1)), 1e-6));
EXPECT(assert_equal(pose2, values.at<Pose3>(Symbol('x', 2)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector3>(Symbol('v', 1)), 1e-6));
EXPECT(assert_equal(v2, values.at<Vector3>(Symbol('v', 2)), 1e-6));
EXPECT(assert_equal(w1, values.at<Vector3>(Symbol('w', 1)), 1e-6));
EXPECT(assert_equal(w2, values.at<Vector3>(Symbol('w', 2)), 1e-6));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testGaussianProcessPriorRot3.cpp
================================================
/**
* @file testGaussianProcessPriorRot3.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gpslam/gp/GaussianProcessPriorRot3.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
/* ************************************************************************** */
TEST(GaussianProcessPriorRot3, Factor) {
const double delta_t = 0.1;
Matrix Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Key key_pose1 = Symbol('x', 1), key_pose2 = Symbol('x', 2);
Key key_vel1 = Symbol('v', 1), key_vel2 = Symbol('v', 2);
GaussianProcessPriorRot3 factor(key_pose1, key_vel1, key_pose2, key_vel2, delta_t, Qc_model);
Rot3 p1, p2;
Vector3 v1, v2;
Matrix actualH1, actualH2, actualH3, actualH4;
Matrix expectH1, expectH2, expectH3, expectH4;
Vector actual, expect;
// test at origin
p1 = Rot3::Ypr(0.0, 0.0, 0.0);
p2 = Rot3::Ypr(0.0, 0.0, 0.0);
v1 = (Vector3() << 0, 0, 0).finished();
v2 = (Vector3() << 0, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test at const rotation of z, w1 = w2 = 1.0;
p1 = Rot3::Ypr(0.0, 0.0, 0.0);
p2 = Rot3::Ypr(0.1, 0.0, 0.0);
v1 = (Vector3() << 0, 0, 1).finished();
v2 = (Vector3() << 0, 0, 1).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// test at const rotation of x, w1 = w2 = 1.0;
p1 = Rot3::Ypr(0.0, 0.0, 0.0);
p2 = Rot3::Ypr(0.0, 0.0, 0.1);
v1 = (Vector3() << 1, 0, 0).finished();
v2 = (Vector3() << 1, 0, 0).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
expectH1 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expect, actual, 1e-6));
EXPECT(assert_equal(expectH1, actualH1, 1e-6));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
// some random stuff just for testing jacobian (error is not zero)
p1 = Rot3::Ypr(-0.1, 1.2, 0.3);
p2 = Rot3::Ypr(2.4, -2.5, 3.7);
v1 = (Vector3() << 2, 3, 1).finished();
v2 = (Vector3() << 1, 3, 8).finished();
actual = factor.evaluateError(p1, v1, p2, v2, actualH1, actualH2, actualH3, actualH4);
expectH1 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
_1, v1, p2, v2, boost::none, boost::none, boost::none, boost::none)), p1, 1e-6);
expectH2 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, _1, p2, v2, boost::none, boost::none, boost::none, boost::none)), v1, 1e-6);
expectH3 = numericalDerivative11(boost::function<Vector(const Rot3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, _1, v2, boost::none, boost::none, boost::none, boost::none)), p2, 1e-6);
expectH4 = numericalDerivative11(boost::function<Vector(const Vector3&)>(
boost::bind(&GaussianProcessPriorRot3::evaluateError, factor,
p1, v1, p2, _1, boost::none, boost::none, boost::none, boost::none)), v2, 1e-6);
EXPECT(assert_equal(expectH1, actualH1, 1e-5));
EXPECT(assert_equal(expectH2, actualH2, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-6));
EXPECT(assert_equal(expectH3, actualH3, 1e-5));
EXPECT(assert_equal(expectH4, actualH4, 1e-6));
}
/* ************************************************************************** */
TEST(GaussianProcessPriorRot3, Optimization) {
/**
* A simple graph:
*
* p1 p2
* | |
* x1 x2
* \ /
* gp
* / \
* v1 v2
*
* p1 and p2 are pose prior factor to fix the poses, gp is the GP factor
* that get correct velocity of v2
*/
noiseModel::Isotropic::shared_ptr model_prior =
noiseModel::Isotropic::Sigma(3, 0.001);
double delta_t = 0.1;
Matrix Qc = 0.01 * Matrix::Identity(3,3);
noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
Rot3 pose1, pose2(Rot3::Ypr(0, 0, 0.1));
Vector3 v1 = (Vector3() << 1, 0, 0).finished();
Vector3 v2 = (Vector3() << 2.0, -0.5, 0.6).finished(); // rnd value
NonlinearFactorGraph graph;
graph.add(PriorFactor<Rot3>(Symbol('x', 1), pose1, model_prior));
graph.add(PriorFactor<Rot3>(Symbol('x', 2), pose2, model_prior));
//graph.add(PriorFactor<Vector6>(Symbol('v', 1), v1, model_prior));
graph.add(GaussianProcessPriorRot3(Symbol('x', 1), Symbol('v', 1),
Symbol('x', 2), Symbol('v', 2), delta_t, Qc_model));
Values init_values;
init_values.insert(Symbol('x', 1), pose1);
init_values.insert(Symbol('v', 1), v1);
init_values.insert(Symbol('x', 2), pose2);
init_values.insert(Symbol('v', 2), v2);
GaussNewtonParams parameters;
GaussNewtonOptimizer optimizer(graph, init_values, parameters);
optimizer.optimize();
Values values = optimizer.values();
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-6);
EXPECT(assert_equal(pose1, values.at<Rot3>(Symbol('x', 1)), 1e-6));
EXPECT(assert_equal(pose2, values.at<Rot3>(Symbol('x', 2)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector3>(Symbol('v', 1)), 1e-6));
EXPECT(assert_equal(v1, values.at<Vector3>(Symbol('v', 2)), 1e-6));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testPose3Utils.cpp
================================================
/**
* @file testPose3Utils.cpp
* @author Jing Dong
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gpslam/gp/Pose3utils.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace gpslam;
/* ************************************************************************** */
// numerical method of jacobians
// see Forster15rss eq. (A.48)
template <class LIE_TYPE>
Matrix numericalLieLeftJacobian(const LIE_TYPE& lie, double dt) {
const size_t dim = LIE_TYPE::dimension;
Vector omega = LIE_TYPE::Logmap(lie);
Matrix J_expect = Matrix(dim, dim);
for (size_t i = 0; i < dim; i++) {
Vector dlogmap = Vector::Zero(dim);
dlogmap(i) = dt;
LIE_TYPE r = LIE_TYPE::Expmap(omega + dlogmap);
J_expect.block(0,i,dim,1) = LIE_TYPE::Logmap(r.compose(lie.inverse())) / dt;
}
return J_expect;
}
template <class LIE_TYPE>
Matrix numericalLieRightJacobian(const LIE_TYPE& lie, double dt) {
const size_t dim = LIE_TYPE::dimension;
Vector omega = LIE_TYPE::Logmap(lie);
Matrix J_expect = Matrix(dim, dim);
for (size_t i = 0; i < dim; i++) {
Vector dlogmap = Vector::Zero(dim);
dlogmap(i) = dt;
LIE_TYPE r = LIE_TYPE::Expmap(omega + dlogmap);
J_expect.block(0,i,dim,1) = LIE_TYPE::Logmap(lie.inverse().compose(r)) / dt;
}
return J_expect;
}
// TODO: why inverse version does not work??
#if 0
// see Barfoot14tro eq. (33), Forster15rss eq. (9)
template <class LIE_TYPE>
Matrix numericalLieLeftJacobian(const LIE_TYPE& lie, double dt) {
const size_t dim = LIE_TYPE::dimension;
Vector omega = LIE_TYPE::Logmap(lie);
Matrix J_expect = Matrix(dim, dim);
for (size_t i = 0; i < dim; i++) {
Vector dlogmap = zero(dim);
dlogmap(i) = dt;
LIE_TYPE dr = LIE_TYPE::Expmap(dlogmap);
J_expect.block(0,i,dim,1) = (LIE_TYPE::Logmap(lie.compose(dr)) - omega) / dt;
}
return J_expect.inverse();
}
template <class LIE_TYPE>
Matrix numericalLieRightJacobian(const LIE_TYPE& lie, double dt) {
const size_t dim = LIE_TYPE::dimension;
Vector omega = LIE_TYPE::Logmap(lie);
Matrix J_expect = Matrix(dim, dim);
for (size_t i = 0; i < dim; i++) {
Vector dlogmap = zero(dim);
dlogmap(i) = dt;
LIE_TYPE dr = LIE_TYPE::Expmap(dlogmap);
J_expect.block(0,i,dim,1) = (LIE_TYPE::Logmap(dr.compose(lie)) - omega) / dt;
}
return J_expect.inverse();
}
#endif
/* ************************************************************************** */
TEST(testPose3Utils, getBodyCentricVelocity) {
Pose3 pose1, pose2;
Vector expect, actual;
double dt = 0.1;
// test at origin
pose1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
pose2 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
actual = getBodyCentricVb(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
expect = (Vector(6) << 0, 0, 0, 0, 0, 0).finished();
actual = getBodyCentricVs(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
// test forward x
pose1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
pose2 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.1, 0.0, 0.0));
expect = (Vector(6) << 0, 0, 0, 1, 0, 0).finished();
actual = getBodyCentricVb(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
expect = (Vector(6) << 0, 0, 0, 1, 0, 0).finished();
actual = getBodyCentricVs(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
// test rotate y
pose1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
pose2 = Pose3(Rot3::Ypr(0.1, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
expect = (Vector(6) << 0, 0, 1, 0, 0, 0).finished();
actual = getBodyCentricVb(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
expect = (Vector(6) << 0, 0, 1, 0, 0, 0).finished();
actual = getBodyCentricVs(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
// test forward x (body-y) at yaw=90 deg
pose1 = Pose3(Rot3::Ypr(M_PI / 2.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
pose2 = Pose3(Rot3::Ypr(M_PI / 2.0, 0.0, 0.0), Point3(0.1, 0.0, 0.0));
expect = (Vector(6) << 0, 0, 0, 0, -1, 0).finished();
actual = getBodyCentricVb(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
expect = (Vector(6) << 0, 0, 0, 1, 0, 0).finished();
actual = getBodyCentricVs(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
// test rotate body+z at yaw=90 deg
pose1 = Pose3(Rot3::Ypr(M_PI / 2.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
pose2 = Pose3(Rot3::Ypr(M_PI / 2.0 + 0.1, 0.0, 0), Point3(0.0, 0.0, 0.0));
expect = (Vector(6) << 0, 0, 1, 0, 0, 0).finished();
actual = getBodyCentricVb(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
expect = (Vector(6) << 0, 0, 1, 0, 0, 0).finished();
actual = getBodyCentricVs(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
// test rotate body+x at yaw=90 deg
pose1 = Pose3(Rot3::Ypr(M_PI / 2.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0));
pose2 = Pose3(Rot3::Ypr(M_PI / 2.0, 0.0, 0.1), Point3(1.0, 0.0, 0.0));
expect = (Vector(6) << 1, 0, 0, 0, 0, 0).finished();
actual = getBodyCentricVb(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
expect = (Vector(6) << 0, 1, 0, 0, 0, 1).finished();
actual = getBodyCentricVs(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
// circle motion
pose1 = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, -1.0, 0.0));
pose2 = Pose3(Rot3::Ypr(M_PI / 2.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0));
expect = (Vector(6) << 0, 0, M_PI/2*10, M_PI/2*10, 0, 0).finished();
actual = getBodyCentricVb(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
expect = (Vector(6) << 0, 0, M_PI/2*10, 0, 0, 0).finished();
actual = getBodyCentricVs(pose1, pose2, dt);
EXPECT(assert_equal(expect, actual, 1e-6));
}
/* ************************************************************************** */
TEST(testPose3Utils, SO3Jacobian) {
Rot3 rotbase;
Matrix J_expect, J_actual;
const double dt = 1e-6;
// left jacobian
rotbase = Rot3::Ypr(0.0, 0.0, 0.0);
J_expect = numericalLieLeftJacobian(rotbase, dt);
J_actual = leftJacobianRot3(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
rotbase = Rot3::Ypr(1.0, 2.0, 3.0);
J_expect = numericalLieLeftJacobian(rotbase, dt);
J_actual = leftJacobianRot3(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
// inverse left jacobian
rotbase = Rot3::Ypr(0.0, 0.0, 0.0);
J_expect = numericalLieLeftJacobian(rotbase, dt).inverse();
J_actual = leftJacobianRot3inv(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
rotbase = Rot3::Ypr(1.0, 2.0, 3.0);
J_expect = numericalLieLeftJacobian(rotbase, dt).inverse();
J_actual = leftJacobianRot3inv(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
// right jacobian
rotbase = Rot3::Ypr(0.0, 0.0, 0.0);
J_expect = numericalLieRightJacobian(rotbase, dt);
J_actual = rightJacobianRot3(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
rotbase = Rot3::Ypr(1.0, 2.0, 3.0);
J_expect = numericalLieRightJacobian(rotbase, dt);
J_actual = rightJacobianRot3(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
// inverse right jacobian
rotbase = Rot3::Ypr(0.0, 0.0, 0.0);
J_expect = numericalLieRightJacobian(rotbase, dt).inverse();
J_actual = rightJacobianRot3inv(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
rotbase = Rot3::Ypr(1.0, 2.0, 3.0);
J_expect = numericalLieRightJacobian(rotbase, dt).inverse();
J_actual = rightJacobianRot3inv(Rot3::Logmap(rotbase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
}
/* ************************************************************************** */
TEST(testPose3Utils, SE3Jacobian) {
Pose3 posebase;
Matrix J_expect, J_actual;
const double dt = 1e-6;
// left jacobian
posebase = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
J_expect = numericalLieLeftJacobian(posebase, dt);
J_actual = leftJacobianPose3(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
posebase = Pose3(Rot3::Ypr(1e-5, 0.0, 1e-5), Point3(0.0, 2e-5, 0.0));
J_expect = numericalLieLeftJacobian(posebase, dt);
J_actual = leftJacobianPose3(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
posebase = Pose3(Rot3::Ypr(1.0, 2.0, 3.0), Point3(4.0, 5.0, 6.0));
J_expect = numericalLieLeftJacobian(posebase, dt);
J_actual = leftJacobianPose3(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
// left inverse jacobian
posebase = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
J_expect = numericalLieLeftJacobian(posebase, dt).inverse();
J_actual = leftJacobianPose3inv(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
posebase = Pose3(Rot3::Ypr(1e-5, 0.0, 1e-5), Point3(0.0, 2e-5, 0.0));
J_expect = numericalLieLeftJacobian(posebase, dt).inverse();
J_actual = leftJacobianPose3inv(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-8));
posebase = Pose3(Rot3::Ypr(1.0, 2.0, 3.0), Point3(4.0, 5.0, 6.0));
J_expect = numericalLieLeftJacobian(posebase, dt).inverse();
J_actual = leftJacobianPose3inv(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-5));
// right jacobian
posebase = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
J_expect = numericalLieRightJacobian(posebase, dt);
J_actual = rightJacobianPose3(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
posebase = Pose3(Rot3::Ypr(1e-5, 0.0, 1e-5), Point3(0.0, 2e-5, 0.0));
J_expect = numericalLieRightJacobian(posebase, dt);
J_actual = rightJacobianPose3(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
posebase = Pose3(Rot3::Ypr(1.0, 2.0, 3.0), Point3(4.0, 5.0, 6.0));
J_expect = numericalLieRightJacobian(posebase, dt);
J_actual = rightJacobianPose3(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
// inverse right jacobian
posebase = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
J_expect = numericalLieRightJacobian(posebase, dt).inverse();
J_actual = rightJacobianPose3inv(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-6));
posebase = Pose3(Rot3::Ypr(1e-5, 0.0, 1e-5), Point3(0.0, 2e-5, 0.0));
J_expect = numericalLieRightJacobian(posebase, dt).inverse();
J_actual = rightJacobianPose3inv(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-8));
posebase = Pose3(Rot3::Ypr(1.0, 2.0, 3.0), Point3(4.0, 5.0, 6.0));
J_expect = numericalLieRightJacobian(posebase, dt).inverse();
J_actual = rightJacobianPose3inv(Pose3::Logmap(posebase));
EXPECT(assert_equal(J_expect, J_actual, 1e-5));
}
/* ************************************************************************** */
TEST(testPose3Utils, SE3velocity) {
// check Anderson15iros eq. (8)
Pose3 posebase, poseadd;
Vector v_expect, v_actual;
Vector dlogmap;
const double dt = 0.01;
// origin, zero vel
posebase = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0));
dlogmap = (Vector(6) << 0, 0, 0, 0, 0, 0).finished() * 1e-4;
poseadd = Pose3::Expmap(Pose3::Logmap(posebase) + dlogmap);
v_expect = getBodyCentricVb(posebase, poseadd, dt);
v_actual = rightJacobianPose3(Pose3::Logmap(posebase)) * dlogmap / dt;
EXPECT(assert_equal(v_expect, v_actual, 1e-6));
v_expect = getBodyCentricVs(posebase, poseadd, dt);
v_actual = leftJacobianPose3(Pose3::Logmap(posebase)) * dlogmap / dt;
EXPECT(assert_equal(v_expect, v_actual, 1e-6));
// origin, non zero vel
posebase = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0));
dlogmap = (Vector(6) << 1, 2, -4, 5, 2, 3).finished() * 1e-4;
poseadd = Pose3::Expmap(Pose3::Logmap(posebase) + dlogmap);
v_expect = getBodyCentricVb(posebase, poseadd, dt);
v_actual = rightJacobianPose3(Pose3::Logmap(posebase)) * dlogmap / dt;
EXPECT(assert_equal(v_expect, v_actual, 1e-6));
v_expect = getBodyCentricVs(posebase, poseadd, dt);
v_actual = leftJacobianPose3(Pose3::Logmap(posebase)) * dlogmap / dt;
EXPECT(assert_equal(v_expect, v_actual, 1e-6));
// rnd pose, non zero vel
posebase = Pose3(Rot3::Ypr(2.4, 1.2, 3.9), Point3(43, -5, 12));
dlogmap = (Vector(6) << 1, 2, -4, 5, 2, 3).finished() * 1e-4;
poseadd = Pose3::Expmap(Pose3::Logmap(posebase) + dlogmap);
v_expect = getBodyCentricVb(posebase, poseadd, dt);
v_actual = rightJacobianPose3(Pose3::Logmap(posebase)) * dlogmap / dt;
EXPECT(assert_equal(v_expect, v_actual, 1e-4));
v_expect = getBodyCentricVs(posebase, poseadd, dt);
v_actual = leftJacobianPose3(Pose3::Logmap(posebase)) * dlogmap / dt;
EXPECT(assert_equal(v_expect, v_actual, 1e-4));
}
/* ************************************************************************** */
// wrap v from convertVbtoVW
Vector3 vWrapperVb(const Vector6& v6, const Pose3& pose) {
Vector3 v,w;
convertVbtoVW(v6, pose, v, w);
return v;
}
// wrap w from convertVbtoVW
Vector3 wWrapperVb(const Vector6& v6, const Pose3& pose) {
Vector3 v,w;
convertVbtoVW(v6, pose, v, w);
return w;
}
TEST(testPose3Utils, convertVelocityVW) {
Pose3 pose;
Vector6 v6exp, v6act;
Vector3 vact, wact, vexp, wexp;
Matrix36 H1vexp, H1vact, H1wexp, H1wact;
Matrix63 H2vexp, H2vact, H2wexp, H2wact;
Matrix6 H2pexp, H2pact;
// zero pose
pose = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
v6exp = (Vector6() << 1, 0, 0, 1, 0, 0).finished();
vexp = Vector3(1, 0, 0), wexp = Vector3(1, 0, 0);
convertVbtoVW(v6exp, pose, vact, wact, H1vact, H1wact);
H1vexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&vWrapperVb, _1, pose)), v6exp, 1e-6);
H1wexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&wWrapperVb, _1, pose)), v6exp, 1e-6);
EXPECT(assert_equal(vexp, vact, 1e-9));
EXPECT(assert_equal(wexp, wact, 1e-9));
EXPECT(assert_equal(H1vexp, H1vact, 1e-6));
EXPECT(assert_equal(H1wexp, H1wact, 1e-6));
v6act = convertVWtoVb(vexp, wexp, pose, H2vact, H2wact, H2pact);
H2vexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, _1, wexp, pose, boost::none, boost::none, boost::none)), vexp, 1e-6);
H2wexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, vexp, _1, pose, boost::none, boost::none, boost::none)), wexp, 1e-6);
H2pexp = numericalDerivative11(boost::function<Vector6(const Pose3&)>(
boost::bind(&convertVWtoVb, vexp, wexp, _1, boost::none, boost::none, boost::none)), pose, 1e-6);
EXPECT(assert_equal(v6exp, v6act, 1e-9));
EXPECT(assert_equal(H2vexp, H2vact, 1e-6));
EXPECT(assert_equal(H2wexp, H2wact, 1e-6));
EXPECT(assert_equal(H2pexp, H2pact, 1e-6));
pose = Pose3(Rot3::Ypr(0.0, 0.0, 0.0), Point3(3.0, 4.0, 5.0));
v6exp = (Vector6() << 1, -2, -3, 1, 2, 3).finished();
vexp = Vector3(1, 2, 3), wexp = Vector3(1, -2, -3);
convertVbtoVW(v6exp, pose, vact, wact, H1vact, H1wact);
H1vexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&vWrapperVb, _1, pose)), v6exp, 1e-6);
H1wexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&wWrapperVb, _1, pose)), v6exp, 1e-6);
EXPECT(assert_equal(vexp, vact, 1e-9));
EXPECT(assert_equal(wexp, wact, 1e-9));
EXPECT(assert_equal(H1vexp, H1vact, 1e-6));
EXPECT(assert_equal(H1wexp, H1wact, 1e-6));
v6act = convertVWtoVb(vexp, wexp, pose, H2vact, H2wact, H2pact);
H2vexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, _1, wexp, pose, boost::none, boost::none, boost::none)), vexp, 1e-6);
H2wexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, vexp, _1, pose, boost::none, boost::none, boost::none)), wexp, 1e-6);
H2pexp = numericalDerivative11(boost::function<Vector6(const Pose3&)>(
boost::bind(&convertVWtoVb, vexp, wexp, _1, boost::none, boost::none, boost::none)), pose, 1e-6);
EXPECT(assert_equal(v6exp, v6act, 1e-9));
EXPECT(assert_equal(H2vexp, H2vact, 1e-6));
EXPECT(assert_equal(H2wexp, H2wact, 1e-6));
EXPECT(assert_equal(H2pexp, H2pact, 1e-6));
// yaw = 90 deg pose
pose = Pose3(Rot3::Ypr(M_PI/2.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
v6exp = (Vector6() << 0, 0, 1, 0, -1, 0).finished();
vexp = Vector3(1, 0, 0), wexp = Vector3(0, 0, 1);
convertVbtoVW(v6exp, pose, vact, wact, H1vact, H1wact);
H1vexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&vWrapperVb, _1, pose)), v6exp, 1e-6);
H1wexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&wWrapperVb, _1, pose)), v6exp, 1e-6);
EXPECT(assert_equal(vexp, vact, 1e-9));
EXPECT(assert_equal(wexp, wact, 1e-9));
EXPECT(assert_equal(H1vexp, H1vact, 1e-6));
EXPECT(assert_equal(H1wexp, H1wact, 1e-6));
v6act = convertVWtoVb(vexp, wexp, pose, H2vact, H2wact, H2pact);
H2vexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, _1, wexp, pose, boost::none, boost::none, boost::none)), vexp, 1e-6);
H2wexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, vexp, _1, pose, boost::none, boost::none, boost::none)), wexp, 1e-6);
H2pexp = numericalDerivative11(boost::function<Vector6(const Pose3&)>(
boost::bind(&convertVWtoVb, vexp, wexp, _1, boost::none, boost::none, boost::none)), pose, 1e-6);
EXPECT(assert_equal(v6exp, v6act, 1e-9));
EXPECT(assert_equal(H2vexp, H2vact, 1e-6));
EXPECT(assert_equal(H2wexp, H2wact, 1e-6));
EXPECT(assert_equal(H2pexp, H2pact, 1e-6));
pose = Pose3(Rot3::Ypr(M_PI/2.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
v6exp = (Vector6() << 0, 0, 1, 4, 3, 0).finished();
vexp = Vector3(-3, 4, 0), wexp = Vector3(0, 0, 1);
convertVbtoVW(v6exp, pose, vact, wact, H1vact, H1wact);
H1vexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&vWrapperVb, _1, pose)), v6exp, 1e-6);
H1wexp = numericalDerivative11(boost::function<Vector3(const Vector6&)>(
boost::bind(&wWrapperVb, _1, pose)), v6exp, 1e-6);
EXPECT(assert_equal(vexp, vact, 1e-9));
EXPECT(assert_equal(wexp, wact, 1e-9));
EXPECT(assert_equal(H1vexp, H1vact, 1e-6));
EXPECT(assert_equal(H1wexp, H1wact, 1e-6));
v6act = convertVWtoVb(vexp, wexp, pose, H2vact, H2wact, H2pact);
H2vexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, _1, wexp, pose, boost::none, boost::none, boost::none)), vexp, 1e-6);
H2wexp = numericalDerivative11(boost::function<Vector6(const Vector3&)>(
boost::bind(&convertVWtoVb, vexp, _1, pose, boost::none, boost::none, boost::none)), wexp, 1e-6);
H2pexp = numericalDerivative11(boost::function<Vector6(const Pose3&)>(
boost::bind(&convertVWtoVb, vexp, wexp, _1, boost::none, boost::none, boost::none)), pose, 1e-6);
EXPECT(assert_equal(v6exp, v6act, 1e-9));
EXPECT(assert_equal(H2vexp, H2vact, 1e-6));
EXPECT(assert_equal(H2wexp, H2wact, 1e-6));
EXPECT(assert_equal(H2pexp, H2pact, 1e-6));
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/gp/tests/testSerializationGP.cpp
================================================
/**
* @file testSerialization.cpp
* @brief test serialization of all factors
* @author Jing Dong
**/
#include <gpslam/gp/GaussianProcessInterpolatorLinear.h>
/*
#include <gpslam/gp/GaussianProcessFactorBasePose3.h>
#include <gpslam/gp/GaussianProcessFactorBasePose3VW.h>
#include <gpslam/gp/GaussianProcessFactorBasePose2.h>
#include <gpslam/gp/GaussianProcessFactorBaseRot3.h>
#include <gpslam/gp/GaussianProcessPriorLinear.h>
#include <gpslam/gp/GaussianProcessPriorPose3.h>
#include <gpslam/gp/GaussianProcessPriorPose3VW.h>
#include <gpslam/gp/GaussianProcessPriorPose2.h>
#include <gpslam/gp/GaussianProcessPriorRot3.h>
*/
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gpslam;
using namespace gtsam::serializationTestHelpers;
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************** */
// data
static SharedNoiseModel Qcmodel_6 = noiseModel::Isotropic::Sigma(6, 0.1);
static SharedNoiseModel Qcmodel_3 = noiseModel::Isotropic::Sigma(3, 0.1);
// bases
static GaussianProcessInterpolatorLinear<6> gp_base_linear(Qcmodel_6, 0.1, 0.04);
/*
static GaussianProcessFactorBasePose3 gp_base_pose3(Qcmodel_6, 0.1, 0.04);
static GaussianProcessFactorBasePose3VW gp_base_pose3vw(Qcmodel_6, 0.1, 0.04);
static GaussianProcessFactorBasePose2 gp_base_pose2(Qcmodel_3, 0.1, 0.04);
static GaussianProcessFactorBaseRot3 gp_base_rot3(Qcmodel_3, 0.1, 0.04);
// factors
static GaussianProcessPriorLinear<6> gp_prior_linear(1, 2, 3, 4, 0.1, Qcmodel_6);
static GaussianProcessPriorPose3 gp_prior_pose3(1, 2, 3, 4, 0.1, Qcmodel_6);
static GaussianProcessPriorPose3VW gp_prior_pose3_vw(1, 2, 3, 4, 5, 6, 0.1, Qcmodel_6);
static GaussianProcessPriorPose2 gp_prior_pose2(1, 2, 3, 4, 0.1, Qcmodel_3);
static GaussianProcessPriorRot3 gp_prior_rot3(1, 2, 3, 4, 0.1, Qcmodel_3);
*/
/* ************************************************************************** */
TEST_UNSAFE(SerializationGP, text) {
EXPECT(equalsObj(gp_base_linear));
/*
EXPECT(equalsObj(gp_base_pose3));
EXPECT(equalsObj(gp_base_pose3vw));
EXPECT(equalsObj(gp_base_pose2));
EXPECT(equalsObj(gp_base_rot3));
EXPECT(equalsObj(gp_prior_linear));
EXPECT(equalsObj(gp_prior_pose3));
EXPECT(equalsObj(gp_prior_pose3_vw));
EXPECT(equalsObj(gp_prior_pose2));
EXPECT(equalsObj(gp_prior_rot3));
*/
}
/* ************************************************************************** */
TEST_UNSAFE(SerializationGP, xml) {
EXPECT(equalsXML(gp_base_linear));
/*
EXPECT(equalsXML(gp_base_pose3));
EXPECT(equalsXML(gp_base_pose3vw));
EXPECT(equalsXML(gp_base_pose2));
EXPECT(equalsXML(gp_base_rot3));
EXPECT(equalsXML(gp_prior_linear));
EXPECT(equalsXML(gp_prior_pose3));
EXPECT(equalsXML(gp_prior_pose3_vw));
EXPECT(equalsXML(gp_prior_pose2));
EXPECT(equalsXML(gp_prior_rot3));
*/
}
/* ************************************************************************** */
TEST_UNSAFE(SerializationGP, binary) {
EXPECT(equalsBinary(gp_base_linear));
/*
EXPECT(equalsBinary(gp_base_pose3));
EXPECT(equalsBinary(gp_base_pose3vw));
EXPECT(equalsBinary(gp_base_pose2));
EXPECT(equalsBinary(gp_base_rot3));
EXPECT(equalsBinary(gp_prior_linear));
EXPECT(equalsBinary(gp_prior_pose3));
EXPECT(equalsBinary(gp_prior_pose3_vw));
EXPECT(equalsBinary(gp_prior_pose2));
EXPECT(equalsBinary(gp_prior_rot3));
*/
}
/* ************************************************************************** */
/* main function */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
================================================
FILE: gpslam/slam/CMakeLists.txt
================================================
# Install headers
file(GLOB slam_headers "*.h")
install(FILES ${slam_headers} DESTINATION include/gpslam/slam)
# Build tests
gtsamAddTestsGlob(slam "tests/*.cpp" "" ${PROJECT_NAME})
================================================
FILE: gpslam/slam/GPInterpolatedAttitudeFactorRot3.h
================================================
/**
* @file GPInterpolatedAttitudeFactorRot3.h
* @author Frank Dellaert, Jing Dong
* @brief Use Acceleration to get Attitude, GP interpolated version
**/
#pragma once
#include <gpslam/gp/GaussianProcessInterpolatorRot3.h>
#include <gtsam/navigation/AttitudeFactor.h>
namespace gpslam {
/**
* Binary factor for an attitude measurement, GP interpolated
*/
class GPInterpolatedAttitudeFactorRot3: public gtsam::NoiseModelFactor4<
gtsam::Rot3, gtsam::Vector3, gtsam::Rot3, gtsam::Vector3>, public gtsam::AttitudeFactor {
private:
// interpolater
GaussianProcessInterpolatorRot3 GPbase_;
typedef GPInterpolatedAttitudeFactorRot3 This;
typedef gtsam::NoiseModelFactor4<gtsam::Rot3, gtsam::Vector3, gtsam::Rot3, gtsam::Vector3> Base;
typedef GaussianProcessInterpolatorRot3 GPBase;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
GPInterpolatedAttitudeFactorRot3() {} /* Default constructor */
/**
* Constructor
* @param nZ measured direction in navigation frame
* @param meas_model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/
GPInterpolatedAttitudeFactorRot3(
gtsam::Key poseKey1, gtsam::Key velKey1, gtsam::Key poseKey2, gtsam::Key velKey2,
double delta_t, double tau, const gtsam::SharedNoiseModel& Qc_model,
const gtsam::SharedNoiseModel& meas_model, const gtsam::Unit3& nZ,
const gtsam::Unit3& bRef = gtsam::Unit3(0, 0, 1)) :
Base(meas_model, poseKey1, velKey1, poseKey2, velKey2),
gtsam::AttitudeFactor(nZ, bRef), GPbase_(Qc_model, delta_t, tau) {
}
virtual ~GPInterpolatedAttitudeFactorRot3() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** factor error */
gtsam::Vector evaluateError(const gtsam::Rot3& pose1, const gtsam::Vector3& vel1,
const gtsam::Rot3& pose2, const gtsam::Vector3& vel2,
boost::optional<gtsam::Matrix&> H1 = boost::none, boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none, boost::optional<gtsam::Matrix&> H4 = boost::none) const {
using namespace gtsam;
// interpolate pose
if (H1 || H2 || H3 || H4) {
Matrix Hint1, Hint2, Hint3, Hint4;
Rot3 pose = GPbase_.interpolatePose(pose1, vel1, pose2, vel2, Hint1, Hint2, Hint3, Hint4);
Matrix Hrot;
Vector err = attitudeError(pose, Hrot);
GPBase::updatePoseJacobians(Hrot, Hint1, Hint2, Hint3, Hint4, H1, H2, H3, H4);
return err;
} else {
Rot3 pose = GPbase_.interpolatePose(pose1, vel1, pose2, vel2);
return attitudeError(pose);
}
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& this->GPbase_.equals(e->GPbase_);
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "GP Interpolated AttitudeFactor" << std::endl;
Base::print("", keyFormatter);
GPbase_.print("Measurement interpolated at: ");
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor4",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(GPbase_);
}
}; // GPInterpolatedAttitudeFactorRot3
} // namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GPInterpolatedAttitudeFactorRot3> : public Testable<gpslam::GPInterpolatedAttitudeFactorRot3> {};
}
================================================
FILE: gpslam/slam/GPInterpolatedGPSFactorPose3.h
================================================
/**
* @file GPInterpolatedGPSFactorPose3.h
* @author Jing Dong
* @brief Translational measurement like GPS, GP interpolated version
* @date Oct 4, 2015
**/
#pragma once
#include <gpslam/gp/GaussianProcessInterpolatorPose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/lexical_cast.hpp>
namespace gpslam {
/**
* Binary factor for an translational measurement like GPS, GP interpolated
*/
class GPInterpolatedGPSFactorPose3: public gtsam::NoiseModelFactor4<gtsam::Pose3,
gtsam::Vector6, gtsam::Pose3, gtsam::Vector6> {
private:
// interpolater
GaussianProcessInterpolatorPose3 GPbase_;
gtsam::Point3 measured_; /** measurement */
boost::optional<gtsam::Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
typedef GPInterpolatedGPSFactorPose3 This;
typedef gtsam::NoiseModelFactor4<gtsam::Pose3, gtsam::Vector6, gtsam::Pose3, gtsam::Vector6> Interpolator;
typedef GaussianProcessInterpolatorPose3 GPBase;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
GPInterpolatedGPSFactorPose3() {} /* Default constructor */
/**
* Constructor
* @param body_P_sensor transformation from body to sensor
*/
GPInterpolatedGPSFactorPose3(const gtsam::Point3& measured_point3,
const gtsam::SharedNoiseModel& meas_model, const gtsam::SharedNoiseModel& Qc_model,
gtsam::Key poseKey1, gtsam::Key velKey1, gtsam::Key poseKey2, gtsam::Key velKey2,
double delta_t, double tau, boost::optional<gtsam::Pose3> body_P_sensor = boost::none) :
Interpolator(meas_model, poseKey1, velKey1, poseKey2, velKey2),
GPbase_(Qc_model, delta_t, tau),
measured_(measured_point3), body_P_sensor_(body_P_sensor) {}
virtual ~GPInterpolatedGPSFactorPose3() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** factor error */
gtsam::Vector evaluateError(const gtsam::Pose3& pose1, const gtsam::Vector6& vel1,
const gtsam::Pose3& pose2, const gtsam::Vector6& vel2,
boost::optional<gtsam::Matrix&> H1 = boost::none, boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none, boost::optional<gtsam::Matrix&> H4 = boost::none) const {
using namespace gtsam;
// interpolate pose
Matrix Hint1, Hint2, Hint3, Hint4;
Pose3 pose;
if (H1 || H2 || H3 || H4)
pose = GPbase_.interpolatePose(pose1, vel1, pose2, vel2, Hint1, Hint2, Hint3, Hint4);
else
pose = GPbase_.interpolatePose(pose1, vel1, pose2, vel2);
Matrix Hpose; // jacobian of pose
Point3 point_err;
if (body_P_sensor_) {
Matrix H0;
point_err = pose.compose(*body_P_sensor_, H0).translation(Hpose) - measured_;
Hpose = Hpose * H0;
GPBase::updatePoseJacobians(Hpose, Hint1, Hint2, Hint3, Hint4, H1, H2, H3, H4);
} else {
point_err = pose.translation(Hpose) - measured_;
GPBase::updatePoseJacobians(Hpose, Hint1, Hint2, Hint3, Hint4, H1, H2, H3, H4);
}
return point_err.vector();
}
/** return the measured */
gtsam::Point3 measured() const {
return measured_;
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol) && GPbase_.equals(e->GPbase_)
&& (this->measured_ - e->measured_).norm() < tol
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "GPSFactor, point = " << measured_ << std::endl;
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
GPbase_.print("Measurement interpolated at: ");
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(GPbase_);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // GPInterpolatedGPSFactorPose3
} // namespace gpslam
/// traits
namespace gtsam {
template<>
struct traits<gpslam::GPInterpolatedGPSFactorPose3> : public Testable<gpslam::GPInterpolatedGPSFactorPose3> {};
}
================================================
FILE: gpslam/slam/GPInterpolatedGPSFactorPose3VW.h
================================================
/**
* @file GPInterpolatedGPSFactorPose3VW.h
* @author Jing Dong
* @brief Translational measurement like GPS, GP interpolated version
* @date Oct 4, 2015
**/
#pragma once
#include <gpslam/gp/GaussianProcessInterpolatorPose3VW.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/lexical_cast.hpp>
namespace gpslam {
/**
* Binary factor for an translational measurement like GPS, GP interpolated
*/
class GPInterpolatedGPSFactorPose3VW: public gtsam::NoiseModelFactor6<gtsam::Pose3,
gtsam::Vector3, gtsam::Vector3, gtsam::Pose3, gtsam::Vector3, gtsam::Vector3> {
private:
// interpolater
GaussianProcessInterpolatorPose3VW GPbase_;
gtsam::Point3 measured_; /** measurement */
boost::optional<gtsam::Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
typedef GPInterpolatedGPSFactorPose3VW This;
typedef gtsam::NoiseModelFactor6<gtsam::Pose3, gtsam::Vector3, gtsam::Vector3,
gtsam::Pose3, gtsam::Vector3, gtsam::Vector3> Interpolator;
typedef GaussianProcessInterpolatorPose3VW GPBase;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
GPInterpolatedGPSFactorPose3VW() {} /* Default constructor */
/**
* Constructor
* @param body_P_sensor transformation from body to sensor
*/
GPInterpolatedGPSFactorPose3VW(const gtsam::Point3& measured_point3,
const gtsam::SharedNoiseModel& meas_model, const gtsam::SharedNoiseModel& Qc_model,
gtsam::Key poseKey1, gtsam::Key velKey1, gtsam::Key omegaKey1,
gtsam::Key poseKey2, gtsam::Key velKey2, gtsam::Key omegaKey2,
double delta_t, double tau, boost::optional<gtsam::Pose3> body_P_sensor = boost::none) :
Interpolator(meas_model, poseKey1, velKey1, omegaKey1, poseKey2, velKey2, omegaKey2),
GPbase_(Qc_model, delta_t, tau),
measured_(measured_point3), body_P_sensor_(body_P_sensor) {
}
virtual ~GPInterpolatedGPSFactorPose3VW() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** factor error */
gtsam::Vector evaluateError(
const gtsam::Pose3& pose1, const gtsam::Vector3& vel1, const gtsam::Vector3& omega1,
const gtsam::Pose3& pose2, const gtsam::Vector3& vel2, const gtsam::Vector3& omega2,
boost::optional<gtsam::Matrix&> H1 = boost::none, boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none, boost::optional<gtsam::Matrix&> H4 = boost::none,
boost::optional<gtsam::Matrix&> H5 = boost::none, boost::optional<gtsam::Matrix&> H6 = boost::none) const {
using namespace gtsam;
// interpolate pose
Matrix Hint1, Hint2, Hint3, Hint4, Hint5, Hint6;
Pose3 pose;
if (H1 || H2 || H3 || H4 || H5 || H6)
pose = GPbase_.interpolatePose(pose1, vel1, omega1, pose2, vel2, omega2,
Hint1, Hint2, Hint3, Hint4, Hint5, Hint6);
else
pose = GPbase_.interpolatePose(pose1, vel1, omega1, pose2, vel2, omega2);
Matrix Hpose; // jacobian of pose
Point3 point_err;
if (body_P_sensor_) {
Matrix H0;
point_err = pose.compose(*body_P_sensor_, H0).translation(Hpose) - measured_;
Hpose = Hpose * H0;
GPBase::updatePoseJacobians(Hpose, Hint1, Hint2, Hint3, Hint4, Hint5, Hint6,
H1, H2, H3, H4, H5, H6);
} else {
point_err = pose.translation(Hpose) - measured_;
GPBase::updatePoseJacobians(Hpose, Hint1, Hint2, Hint3, Hint4, Hint5, Hint6,
H1, H2, H3, H4, H5, H6);
}
return point_err.vector();
}
/** return the measured */
gtsam::Point3 measured() const {
return measured_;
}
/** equals specialized to this factor */
virtual bool equals(const gtsam::NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol) && GPbase_.equals(e->GPbase_)
&& (this->measured_ - e->measured_).norm() < tol
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** print contents */
void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
std::cout << s << "GPSFa
gitextract_xmqbx817/
├── .gitignore
├── CHANGELOG
├── CMakeLists.txt
├── LICENSE
├── README.md
├── gpslam/
│ ├── CMakeLists.txt
│ ├── gp/
│ │ ├── CMakeLists.txt
│ │ ├── GPutils.cpp
│ │ ├── GPutils.h
│ │ ├── GaussianProcessInterpolatorLinear.h
│ │ ├── GaussianProcessInterpolatorPose2.h
│ │ ├── GaussianProcessInterpolatorPose3.h
│ │ ├── GaussianProcessInterpolatorPose3VW.h
│ │ ├── GaussianProcessInterpolatorRot3.h
│ │ ├── GaussianProcessPriorLinear.h
│ │ ├── GaussianProcessPriorPose2.h
│ │ ├── GaussianProcessPriorPose3.h
│ │ ├── GaussianProcessPriorPose3VW.h
│ │ ├── GaussianProcessPriorRot3.h
│ │ ├── Pose3utils.cpp
│ │ ├── Pose3utils.h
│ │ └── tests/
│ │ ├── testGaussianProcessInterpolatorLinear.cpp
│ │ ├── testGaussianProcessInterpolatorPose2.cpp
│ │ ├── testGaussianProcessInterpolatorPose3.cpp
│ │ ├── testGaussianProcessInterpolatorPose3VW.cpp
│ │ ├── testGaussianProcessInterpolatorRot3.cpp
│ │ ├── testGaussianProcessPriorLinear.cpp
│ │ ├── testGaussianProcessPriorPose2.cpp
│ │ ├── testGaussianProcessPriorPose3.cpp
│ │ ├── testGaussianProcessPriorPose3VW.cpp
│ │ ├── testGaussianProcessPriorRot3.cpp
│ │ ├── testPose3Utils.cpp
│ │ └── testSerializationGP.cpp
│ └── slam/
│ ├── CMakeLists.txt
│ ├── GPInterpolatedAttitudeFactorRot3.h
│ ├── GPInterpolatedGPSFactorPose3.h
│ ├── GPInterpolatedGPSFactorPose3VW.h
│ ├── GPInterpolatedProjectionFactorPose3.h
│ ├── GPInterpolatedRangeFactor2DLinear.h
│ ├── GPInterpolatedRangeFactorPose2.h
│ ├── GPInterpolatedRangeFactorPose3.h
│ ├── OdometryFactor2DLinear.h
│ ├── RangeBearingFactor2DLinear.h
│ ├── RangeFactor2DLinear.h
│ ├── RangeFactorPose2.h
│ └── tests/
│ ├── testGPInterpolatedGPSFactorPose3.cpp
│ ├── testGPInterpolatedGPSFactorPose3VW.cpp
│ ├── testGPInterpolatedProjectionFactorPose3.cpp
│ ├── testGPInterpolatedRangeFactor2DLinear.cpp
│ ├── testGPInterpolatedRangeFactorPose2.cpp
│ ├── testGPInterpolatedRangeFactorPose3.cpp
│ ├── testOdometryFactor2DLinear.cpp
│ ├── testRangeBearingFactor2DLinear.cpp
│ ├── testRangeFactor2DLinear.cpp
│ └── testSerializationSLAM.cpp
├── gpslam.h
└── matlab/
├── AHRSexample.m
├── GPAHRSexample.m
├── PlazaPose2.m
├── data/
│ ├── MOCAP_POSE_DATA_matlab.txt
│ ├── Plaza1.mat
│ ├── Plaza2.mat
│ └── RAW_IMU_DATA_matlab.txt
└── range_measure_fit.m
SYMBOL INDEX (142 symbols across 48 files)
FILE: gpslam.h
function namespace (line 24) | namespace gpslam {
FILE: gpslam/gp/GPutils.cpp
type gpslam (line 13) | namespace gpslam {
function Matrix (line 16) | Matrix getQc(const SharedNoiseModel& Qc_model) {
FILE: gpslam/gp/GPutils.h
function namespace (line 17) | namespace gpslam {
FILE: gpslam/gp/GaussianProcessInterpolatorLinear.h
function namespace (line 18) | namespace gpslam {
function namespace (line 172) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessInterpolatorPose2.h
function namespace (line 15) | namespace gpslam {
function namespace (line 153) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessInterpolatorPose3.h
function namespace (line 16) | namespace gpslam {
function namespace (line 165) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessInterpolatorPose3VW.h
function namespace (line 16) | namespace gpslam {
function namespace (line 191) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessInterpolatorRot3.h
function namespace (line 15) | namespace gpslam {
function namespace (line 147) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessPriorLinear.h
function namespace (line 18) | namespace gpslam {
function namespace (line 124) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessPriorPose2.h
function namespace (line 22) | namespace gpslam {
function namespace (line 117) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessPriorPose3.h
function namespace (line 24) | namespace gpslam {
function namespace (line 134) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessPriorPose3VW.h
function namespace (line 23) | namespace gpslam {
function namespace (line 154) | namespace gtsam {
FILE: gpslam/gp/GaussianProcessPriorRot3.h
function namespace (line 22) | namespace gpslam {
function namespace (line 115) | namespace gtsam {
FILE: gpslam/gp/Pose3utils.cpp
type gpslam (line 13) | namespace gpslam {
function Vector6 (line 17) | Vector6 getBodyCentricVb(const Pose3& pose1, const Pose3& pose2, doubl...
function Vector6 (line 22) | Vector6 getBodyCentricVs(const Pose3& pose1, const Pose3& pose2, doubl...
function convertVbtoVW (line 27) | void convertVbtoVW(const Vector6& v6, const Pose3& pose, Vector3& v, V...
function Vector6 (line 47) | Vector6 convertVWtoVb(const Vector3& v, const Vector3& w, const Pose3&...
function Matrix3 (line 68) | Matrix3 leftJacobianPose3Q(const Vector6& xi) {
function Matrix3 (line 92) | Matrix3 rightJacobianPose3Q(const Vector6& xi) {
function Matrix6 (line 117) | Matrix6 leftJacobianPose3(const Vector6& xi) {
function Matrix6 (line 127) | Matrix6 leftJacobianPose3inv(const Vector6& xi) {
function Matrix3 (line 137) | Matrix3 leftJacobianRot3(const Vector3& omega) {
function Matrix3 (line 152) | Matrix3 leftJacobianRot3inv(const Vector3& omega) {
function Matrix6 (line 167) | Matrix6 jacobianMethodNumercialDiff(boost::function<Matrix6(const Vect...
function Matrix6 (line 182) | Matrix6 rightJacobianPose3(const Vector6& xi) {
function Matrix6 (line 192) | Matrix6 rightJacobianPose3inv(const Vector6& xi) {
function Matrix3 (line 203) | Matrix3 rightJacobianRot3(const Vector3& omega) {
function Matrix3 (line 215) | Matrix3 rightJacobianRot3inv(const Vector3& omega) {
FILE: gpslam/gp/Pose3utils.h
function namespace (line 16) | namespace gpslam {
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorLinear.cpp
function TEST (line 25) | TEST(GaussianProcessInterpolatorLinear, equals) {
function TEST (line 47) | TEST(GaussianProcessInterpolatorLinear, interpolatePose) {
function main (line 162) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorPose2.cpp
function TEST (line 22) | TEST(GaussianProcessInterpolatorPose2, interpolatePose) {
function main (line 141) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorPose3.cpp
function TEST (line 22) | TEST(GaussianProcessInterpolatorPose3, interpolatePose) {
function main (line 141) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorPose3VW.cpp
function Pose3 (line 21) | Pose3 interWrapper(const GaussianProcessInterpolatorPose3VW& interpolater,
function TEST (line 28) | TEST(GaussianProcessInterpolatorPose3VW, interpolatePose) {
function main (line 154) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessInterpolatorRot3.cpp
function TEST (line 22) | TEST(GaussianProcessInterpolatorRot3, interpolatePose) {
function main (line 141) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessPriorLinear.cpp
function TEST (line 30) | TEST(GaussianProcessPriorLinear, Factor) {
function TEST (line 144) | TEST(GaussianProcessPriorLinear, Optimization) {
function main (line 206) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessPriorPose2.cpp
function TEST (line 27) | TEST(GaussianProcessPriorPose2, Factor) {
function TEST (line 146) | TEST(GaussianProcessPriorPose2, Optimization) {
function main (line 198) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessPriorPose3.cpp
function TEST (line 27) | TEST(GaussianProcessPriorPose3Test, Factor) {
function TEST (line 146) | TEST(GaussianProcessPriorPose3Test, Optimization) {
function main (line 199) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessPriorPose3VW.cpp
function Vector (line 26) | Vector errorWrapper(const GaussianProcessPriorPose3VW& factor,
function TEST (line 33) | TEST(GaussianProcessPriorPose3VW, Factor) {
function TEST (line 161) | TEST(GaussianProcessPriorPose3VW, Optimization) {
function main (line 218) | int main() {
FILE: gpslam/gp/tests/testGaussianProcessPriorRot3.cpp
function TEST (line 27) | TEST(GaussianProcessPriorRot3, Factor) {
function TEST (line 145) | TEST(GaussianProcessPriorRot3, Optimization) {
function main (line 198) | int main() {
FILE: gpslam/gp/tests/testPose3Utils.cpp
function Matrix (line 30) | Matrix numericalLieLeftJacobian(const LIE_TYPE& lie, double dt) {
function Matrix (line 44) | Matrix numericalLieRightJacobian(const LIE_TYPE& lie, double dt) {
function Matrix (line 61) | Matrix numericalLieLeftJacobian(const LIE_TYPE& lie, double dt) {
function Matrix (line 75) | Matrix numericalLieRightJacobian(const LIE_TYPE& lie, double dt) {
function TEST (line 90) | TEST(testPose3Utils, getBodyCentricVelocity) {
function TEST (line 167) | TEST(testPose3Utils, SO3Jacobian) {
function TEST (line 218) | TEST(testPose3Utils, SE3Jacobian) {
function TEST (line 289) | TEST(testPose3Utils, SE3velocity) {
function Vector3 (line 332) | Vector3 vWrapperVb(const Vector6& v6, const Pose3& pose) {
function Vector3 (line 339) | Vector3 wWrapperVb(const Vector6& v6, const Pose3& pose) {
function TEST (line 345) | TEST(testPose3Utils, convertVelocityVW) {
function main (line 458) | int main() {
FILE: gpslam/gp/tests/testSerializationGP.cpp
function TEST_UNSAFE (line 60) | TEST_UNSAFE(SerializationGP, text) {
function TEST_UNSAFE (line 77) | TEST_UNSAFE(SerializationGP, xml) {
function TEST_UNSAFE (line 94) | TEST_UNSAFE(SerializationGP, binary) {
function main (line 112) | int main() {
FILE: gpslam/slam/GPInterpolatedAttitudeFactorRot3.h
function namespace (line 14) | namespace gpslam {
function namespace (line 117) | namespace gtsam {
FILE: gpslam/slam/GPInterpolatedGPSFactorPose3.h
function namespace (line 19) | namespace gpslam {
function namespace (line 138) | namespace gtsam {
FILE: gpslam/slam/GPInterpolatedGPSFactorPose3VW.h
function namespace (line 19) | namespace gpslam {
function namespace (line 146) | namespace gtsam {
FILE: gpslam/slam/GPInterpolatedProjectionFactorPose3.h
function namespace (line 18) | namespace gpslam {
function namespace (line 203) | namespace gtsam {
FILE: gpslam/slam/GPInterpolatedRangeFactor2DLinear.h
function namespace (line 16) | namespace gpslam {
function namespace (line 125) | namespace gtsam {
FILE: gpslam/slam/GPInterpolatedRangeFactorPose2.h
function namespace (line 18) | namespace gpslam {
function namespace (line 143) | namespace gtsam {
FILE: gpslam/slam/GPInterpolatedRangeFactorPose3.h
function namespace (line 18) | namespace gpslam {
function namespace (line 144) | namespace gtsam {
FILE: gpslam/slam/OdometryFactor2DLinear.h
function namespace (line 19) | namespace gpslam {
function namespace (line 118) | namespace gtsam {
FILE: gpslam/slam/RangeBearingFactor2DLinear.h
function namespace (line 14) | namespace gpslam {
function namespace (line 126) | namespace gtsam {
FILE: gpslam/slam/RangeFactor2DLinear.h
function namespace (line 14) | namespace gpslam {
function namespace (line 92) | namespace gtsam {
FILE: gpslam/slam/RangeFactorPose2.h
function namespace (line 13) | namespace gpslam {
FILE: gpslam/slam/tests/testGPInterpolatedGPSFactorPose3.cpp
function Vector (line 31) | inline Vector errorWrapper(const GPSFactor& factor,
function TEST (line 38) | TEST(GPInterpolatedGPSFactorPose3, error) {
function TEST (line 181) | TEST(GPInterpolatedGPSFactorPose3, optimization) {
function main (line 259) | int main() {
FILE: gpslam/slam/tests/testGPInterpolatedGPSFactorPose3VW.cpp
function Vector (line 31) | inline Vector errorWrapper(const GPSFactor& factor,
function TEST (line 38) | TEST(GPInterpolatedGPSFactorPose3VW, error) {
function TEST (line 278) | TEST(GPInterpolatedGPSFactorPose3VW, optimization) {
function main (line 359) | int main() {
FILE: gpslam/slam/tests/testGPInterpolatedProjectionFactorPose3.cpp
function Vector (line 31) | inline Vector errorWrapper(const ProjectionFactor& factor,
function TEST (line 38) | TEST(GPInterpolatedProjectionFactorPose3, projection) {
function TEST (line 178) | TEST(GPInterpolatedProjectionFactorPose3, optimization) {
function main (line 268) | int main() {
FILE: gpslam/slam/tests/testGPInterpolatedRangeFactor2DLinear.cpp
function Vector (line 31) | inline Vector errorWrapper(const RangeFactor& factor,
function TEST (line 38) | TEST(GPInterpolatedRangeFactor2DLinear, range) {
function TEST (line 229) | TEST(GPInterpolatedRangeFactor2DLinear, optimization) {
function main (line 321) | int main() {
FILE: gpslam/slam/tests/testGPInterpolatedRangeFactorPose2.cpp
function Vector (line 31) | inline Vector errorWrapper(const RangeFactor& factor,
function TEST (line 38) | TEST(GPInterpolatedRangeFactorPose2, range) {
function TEST (line 200) | TEST(GPInterpolatedRangeFactorPose2, optimization) {
function main (line 287) | int main() {
FILE: gpslam/slam/tests/testGPInterpolatedRangeFactorPose3.cpp
function Vector (line 31) | inline Vector errorWrapper(const RangeFactor& factor,
function TEST (line 38) | TEST(GPInterpolatedRangeFactorPose3, range) {
function TEST (line 177) | TEST(GPInterpolatedRangeFactorPose3, optimization) {
function main (line 264) | int main() {
FILE: gpslam/slam/tests/testOdometryFactor2DLinear.cpp
function TEST (line 27) | TEST(OdometryFactor2DLinear, Factor) {
function main (line 84) | int main() {
FILE: gpslam/slam/tests/testRangeBearingFactor2DLinear.cpp
function TEST (line 27) | TEST(RangeBearingFactor2DLinear, Factor) {
function TEST (line 72) | TEST(RangeBearingFactor2DLinear, Optimization) {
function main (line 137) | int main() {
FILE: gpslam/slam/tests/testRangeFactor2DLinear.cpp
function TEST (line 27) | TEST(RangeFactor2DLinear, Factor) {
function TEST (line 80) | TEST(RangeFactor2DLinear, Optimization) {
function main (line 144) | int main() {
FILE: gpslam/slam/tests/testSerializationSLAM.cpp
function TEST (line 55) | TEST(SerializationSLAM, text) {
function TEST (line 67) | TEST(SerializationSLAM, xml) {
function TEST (line 79) | TEST(SerializationSLAM, binary) {
function main (line 92) | int main() {
Condensed preview — 64 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,683K chars).
[
{
"path": ".gitignore",
"chars": 18,
"preview": "*.DS_Store\nbuild*\n"
},
{
"path": "CHANGELOG",
"chars": 194,
"preview": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for GP-SLAM\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n\n0.1.0 (2017-08-25)\n--"
},
{
"path": "CMakeLists.txt",
"chars": 1583,
"preview": "cmake_minimum_required(VERSION 2.6)\nenable_testing()\nproject(gpslam CXX C)\n\n# version indicator\nset(GPSLAM_VERSION_MAJOR"
},
{
"path": "LICENSE",
"chars": 1531,
"preview": "Copyright (c) 2016, Georgia Tech Research Corporation\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nRedistribution an"
},
{
"path": "README.md",
"chars": 2826,
"preview": "GP-SLAM\n===================================================\nGP-SLAM is a library implenmenting sparse Gaussian process ("
},
{
"path": "gpslam/CMakeLists.txt",
"chars": 980,
"preview": "# We split the library in to separate subfolders, each containing\n# tests, timing, and an optional convenience library.\n"
},
{
"path": "gpslam/gp/CMakeLists.txt",
"chars": 175,
"preview": "# Install headers\nfile(GLOB gp_headers \"*.h\")\ninstall(FILES ${gp_headers} DESTINATION include/gpslam/gp)\n\n# Build tests\n"
},
{
"path": "gpslam/gp/GPutils.cpp",
"chars": 559,
"preview": "/**\n * @file GPutils.cpp\n * @brief GP utils, calculation of Qc, Q, Lamda matrices etc.\n * @author Xinyan Yan, Jing D"
},
{
"path": "gpslam/gp/GPutils.h",
"chars": 2353,
"preview": "/**\n * @file GPutils.h\n * @brief GP utils, calculation of Qc, Q, Lamda matrices etc.\n * @author Xinyan Yan, Jing Don"
},
{
"path": "gpslam/gp/GaussianProcessInterpolatorLinear.h",
"chars": 5887,
"preview": "/**\n * @file GaussianProcessInterpolatorLinear.h\n * @brief Base and utils for Gaussian Process Interpolated measurement "
},
{
"path": "gpslam/gp/GaussianProcessInterpolatorPose2.h",
"chars": 5296,
"preview": "/**\n * @file GaussianProcessInterpolatorPose2.h\n * @brief Base and utils for Gaussian Process Interpolated measurement f"
},
{
"path": "gpslam/gp/GaussianProcessInterpolatorPose3.h",
"chars": 5789,
"preview": "/**\n * @file GaussianProcessInterpolatorPose3.h\n * @brief Base and utils for Gaussian Process Interpolated measurement f"
},
{
"path": "gpslam/gp/GaussianProcessInterpolatorPose3VW.h",
"chars": 6994,
"preview": "/**\n * @file GaussianProcessInterpolatorPose3VW.h\n * @brief Base and utils for Gaussian Process Interpolated measurement"
},
{
"path": "gpslam/gp/GaussianProcessInterpolatorRot3.h",
"chars": 5232,
"preview": "/**\n * @file GaussianProcessInterpolatorRot3.h\n * @brief Base and utils for Gaussian Process Interpolated measurement fa"
},
{
"path": "gpslam/gp/GaussianProcessPriorLinear.h",
"chars": 4156,
"preview": "/**\n * @file GaussianProcessPriorLinear.h\n * @brief Linear GP prior, see Barfoot14rss\n * @author Xinyan Yan, Jing Do"
},
{
"path": "gpslam/gp/GaussianProcessPriorPose2.h",
"chars": 3819,
"preview": "/**\n * @file GaussianProcessPriorPose2.h\n * @brief Pose2 GP prior\n * @author Jing Dong\n **/\n\n#pragma once\n\n#include "
},
{
"path": "gpslam/gp/GaussianProcessPriorPose3.h",
"chars": 4250,
"preview": "/**\n * @file GaussianProcessPriorPose3.h\n * @brief Pose3 GP prior\n * @author Xinyan Yan, Jing Dong\n **/\n\n#pragma onc"
},
{
"path": "gpslam/gp/GaussianProcessPriorPose3VW.h",
"chars": 5201,
"preview": "/**\n * @file GaussianProcessPriorPose3VW.h\n * @brief Pose3 GP prior, use V/W velocity representation\n * @author Xiny"
},
{
"path": "gpslam/gp/GaussianProcessPriorRot3.h",
"chars": 3782,
"preview": "/**\n * @file GaussianProcessPriorRot3.h\n * @brief Rot3 GP prior\n * @author Jing Dong\n **/\n\n#pragma once\n\n#include <g"
},
{
"path": "gpslam/gp/Pose3utils.cpp",
"chars": 8509,
"preview": "/**\n* @file Pose3utils.cpp\n* @author Jing Dong\n**/\n\n#include <gpslam/gp/Pose3utils.h>\n\n#include <cmath>\n\nusing namesp"
},
{
"path": "gpslam/gp/Pose3utils.h",
"chars": 2606,
"preview": "/**\n * @file Pose3utils.h\n * @brief Pose3 GP utils, mainly jacobians of expmap/logmap\n * @author Xinyan Yan, Jing Do"
},
{
"path": "gpslam/gp/tests/testGaussianProcessInterpolatorLinear.cpp",
"chars": 7765,
"preview": "/**\n* @file testGaussianProcessInterpolatorLinear.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#"
},
{
"path": "gpslam/gp/tests/testGaussianProcessInterpolatorPose2.cpp",
"chars": 6965,
"preview": "/**\n* @file testGaussianProcessInterpolatorPose2.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#i"
},
{
"path": "gpslam/gp/tests/testGaussianProcessInterpolatorPose3.cpp",
"chars": 7376,
"preview": "/**\n* @file testGaussianProcessInterpolatorPose3.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#i"
},
{
"path": "gpslam/gp/tests/testGaussianProcessInterpolatorPose3VW.cpp",
"chars": 7941,
"preview": "/**\n* @file testGaussianProcessInterpolatorPose3VW.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n"
},
{
"path": "gpslam/gp/tests/testGaussianProcessInterpolatorRot3.cpp",
"chars": 6981,
"preview": "/**\n* @file testGaussianProcessInterpolatorRot3.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#in"
},
{
"path": "gpslam/gp/tests/testGaussianProcessPriorLinear.cpp",
"chars": 9457,
"preview": "/**\n* @file testGaussianProcessPriorLinear.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include"
},
{
"path": "gpslam/gp/tests/testGaussianProcessPriorPose2.cpp",
"chars": 9184,
"preview": "/**\n* @file testGaussianProcessPriorPose2.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include "
},
{
"path": "gpslam/gp/tests/testGaussianProcessPriorPose3.cpp",
"chars": 9741,
"preview": "/**\n* @file testGaussianProcessPriorPose3.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include "
},
{
"path": "gpslam/gp/tests/testGaussianProcessPriorPose3VW.cpp",
"chars": 10742,
"preview": "/**\n* @file testGaussianProcessPriorPose3VW.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#includ"
},
{
"path": "gpslam/gp/tests/testGaussianProcessPriorRot3.cpp",
"chars": 9280,
"preview": "/**\n* @file testGaussianProcessPriorRot3.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include <"
},
{
"path": "gpslam/gp/tests/testPose3Utils.cpp",
"chars": 19813,
"preview": "/**\n* @file testPose3Utils.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include <gtsam/base/Mat"
},
{
"path": "gpslam/gp/tests/testSerializationGP.cpp",
"chars": 4163,
"preview": "/**\n* @file testSerialization.cpp\n* @brief test serialization of all factors\n* @author Jing Dong\n**/\n\n#include <gpsla"
},
{
"path": "gpslam/slam/CMakeLists.txt",
"chars": 183,
"preview": "# Install headers\nfile(GLOB slam_headers \"*.h\")\ninstall(FILES ${slam_headers} DESTINATION include/gpslam/slam)\n\n# Build "
},
{
"path": "gpslam/slam/GPInterpolatedAttitudeFactorRot3.h",
"chars": 3990,
"preview": "/**\n * @file GPInterpolatedAttitudeFactorRot3.h\n * @author Frank Dellaert, Jing Dong\n * @brief Use Acceleration to g"
},
{
"path": "gpslam/slam/GPInterpolatedGPSFactorPose3.h",
"chars": 4853,
"preview": "/**\n * @file GPInterpolatedGPSFactorPose3.h\n * @author Jing Dong\n * @brief Translational measurement like GPS, GP in"
},
{
"path": "gpslam/slam/GPInterpolatedGPSFactorPose3VW.h",
"chars": 5341,
"preview": "/**\n * @file GPInterpolatedGPSFactorPose3VW.h\n * @author Jing Dong\n * @brief Translational measurement like GPS, GP "
},
{
"path": "gpslam/slam/GPInterpolatedProjectionFactorPose3.h",
"chars": 7445,
"preview": "/**\n * @file GPInterpolatedProjectionFactorPose3.h\n * @brief GP Interpolation projection measurement factor for Pose3\n *"
},
{
"path": "gpslam/slam/GPInterpolatedRangeFactor2DLinear.h",
"chars": 4118,
"preview": "/**\n * @file GPInterpolatedRangeFactor2DLinear.h\n * @brief range factor for 2D linear pose, interpolated\n * @author "
},
{
"path": "gpslam/slam/GPInterpolatedRangeFactorPose2.h",
"chars": 5039,
"preview": "/**\n * @file GPInterpolatedRangeFactorPose2.H\n * @brief range factor for SE(2) pose, interpolated\n * @author Jing Do"
},
{
"path": "gpslam/slam/GPInterpolatedRangeFactorPose3.h",
"chars": 5055,
"preview": "/**\n * @file GPInterpolatedRangeFactorPose3.H\n * @brief range factor for SE(3) pose, interpolated\n * @author Frank D"
},
{
"path": "gpslam/slam/OdometryFactor2DLinear.h",
"chars": 3597,
"preview": "/**\n* @file OdometryFactor2DLinear.H\n* @brief 2-way odometry factor, 2D linear pose version\n* @author Xinyan Yan, Ji"
},
{
"path": "gpslam/slam/RangeBearingFactor2DLinear.h",
"chars": 3865,
"preview": "/**\n * @file RangeBearingFactor2DLinear.h\n * @brief range + bearing factor for 2D linear pose\n * @author Xinyan Yan,"
},
{
"path": "gpslam/slam/RangeFactor2DLinear.h",
"chars": 2662,
"preview": "/**\n * @file RangeFactor2DLinear.h\n * @brief range factor for 2D linear pose\n * @author Xinyan Yan, Jing Dong\n **/\n\n"
},
{
"path": "gpslam/slam/RangeFactorPose2.h",
"chars": 336,
"preview": "/**\n * @file RangeFactorPose2.h\n * @brief range factor pose2\n * @author Jing Dong\n **/\n\n#pragma once\n\n#include <gtsa"
},
{
"path": "gpslam/slam/tests/testGPInterpolatedGPSFactorPose3.cpp",
"chars": 11689,
"preview": "/**\n* @file testGPInterpolatedGPSFactorPose3.cpp\n* @author Jing Dong\n* @date Oct 4, 2015\n**/\n\n#include <CppUnitLite/T"
},
{
"path": "gpslam/slam/tests/testGPInterpolatedGPSFactorPose3VW.cpp",
"chars": 18216,
"preview": "/**\n* @file testGPInterpolatedGPSFactorPose3VW.cpp\n* @author Jing Dong\n* @date Nov 21, 2015\n**/\n\n#include <CppUnitLit"
},
{
"path": "gpslam/slam/tests/testGPInterpolatedProjectionFactorPose3.cpp",
"chars": 12259,
"preview": "/**\n* @file testGPInterpolatedProjectionFactorPose3.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n"
},
{
"path": "gpslam/slam/tests/testGPInterpolatedRangeFactor2DLinear.cpp",
"chars": 15043,
"preview": "/**\n* @file testGPInterpolatedRangeFactor2DLinear.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#"
},
{
"path": "gpslam/slam/tests/testGPInterpolatedRangeFactorPose2.cpp",
"chars": 12754,
"preview": "/**\n* @file testGPInterpolatedRangeFactorPose2.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#inc"
},
{
"path": "gpslam/slam/tests/testGPInterpolatedRangeFactorPose3.cpp",
"chars": 12050,
"preview": "/**\n* @file testGPInterpolatedRangeFactorPose3.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#inc"
},
{
"path": "gpslam/slam/tests/testOdometryFactor2DLinear.cpp",
"chars": 3229,
"preview": "/**\n* @file testRangeFactor2DLinear.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include <gtsam"
},
{
"path": "gpslam/slam/tests/testRangeBearingFactor2DLinear.cpp",
"chars": 5262,
"preview": "/**\n* @file testRangeBearingFactor2DLinear.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include"
},
{
"path": "gpslam/slam/tests/testRangeFactor2DLinear.cpp",
"chars": 5301,
"preview": "/**\n* @file testRangeFactor2DLinear.cpp\n* @author Jing Dong\n**/\n\n#include <CppUnitLite/TestHarness.h>\n\n#include <gtsam"
},
{
"path": "gpslam/slam/tests/testSerializationSLAM.cpp",
"chars": 3683,
"preview": "/**\n* @file testSerialization.cpp\n* @brief test serialization of all factors\n* @author Jing Dong\n**/\n\n#include <gpsla"
},
{
"path": "gpslam.h",
"chars": 9312,
"preview": "// GP-SLAM wrapper\n\n// delearation\nclass gtsam::Vector6;\nclass gtsam::Vector3;\nclass gtsam::Rot2;\nclass gtsam::Point2;\nc"
},
{
"path": "matlab/AHRSexample.m",
"chars": 6660,
"preview": "% AHRS example without using GP\n% Jing 02.08.2016\n\nclear\nclose all\nimport gtsam.*\nimport gpslam.*\n\n%% settings\nuseGyro ="
},
{
"path": "matlab/GPAHRSexample.m",
"chars": 10414,
"preview": "% AHRS example with GP, fuse asynchronous accelerometer and gyroscope data\n% @author Jing Dong \n% @date 02.15.2016\n\nclea"
},
{
"path": "matlab/PlazaPose2.m",
"chars": 9788,
"preview": "% 2D SLAM example using Range + Odometry data, Plaza datasets\n% dataset format: http://www.frc.ri.cmu.edu/projects/emerg"
},
{
"path": "matlab/data/MOCAP_POSE_DATA_matlab.txt",
"chars": 574858,
"preview": "3606 0.004587 -0.094220 -1.468240 0.866052 0.028918 0.049933 0.997198 -0.047600\n3607 0.013381 -0.094171 -1.468645 0.8661"
},
{
"path": "matlab/data/RAW_IMU_DATA_matlab.txt",
"chars": 710681,
"preview": "4814 0.000000 -0.003638 -0.002836 -0.009831 0.766352 0.880101 10.027621\n4815 0.004458 -0.003813 -0.002984 -0.009896 0.75"
},
{
"path": "matlab/range_measure_fit.m",
"chars": 3215,
"preview": "function [range_trans, range_outlier_mask] = range_measure_fit(GT, TL, TD)\r\n%RANGE_MEASURE_FIT fit range measurment bias"
}
]
// ... and 2 more files (download for full content)
About this extraction
This page contains the full source code of the gtrll/gpslam GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 64 files (1.6 MB), approximately 801.6k tokens, and a symbol index with 142 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.