ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
test/CMakeLists.txt Normal file
View File

@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.10...3.18)
# Note: add_subdirectory(googletest ...) is called in basalt-headers
include_directories(../thirdparty/basalt-headers/test/include)
add_executable(test_spline_opt src/test_spline_opt.cpp)
target_link_libraries(test_spline_opt gtest gtest_main basalt)
add_executable(test_vio src/test_vio.cpp)
target_link_libraries(test_vio gtest gtest_main basalt)
add_executable(test_nfr src/test_nfr.cpp)
target_link_libraries(test_nfr gtest gtest_main basalt)
add_executable(test_qr src/test_qr.cpp)
target_link_libraries(test_qr gtest gtest_main basalt)
add_executable(test_linearization src/test_linearization.cpp)
target_link_libraries(test_linearization gtest gtest_main basalt)
add_executable(test_patch src/test_patch.cpp)
target_link_libraries(test_patch gtest gtest_main basalt)
enable_testing()
include(GoogleTest)
#gtest_discover_tests(test_spline_opt DISCOVERY_TIMEOUT 60)
#gtest_discover_tests(test_vio DISCOVERY_TIMEOUT 60)
#gtest_discover_tests(test_nfr DISCOVERY_TIMEOUT 60)
gtest_add_tests(TARGET test_spline_opt AUTO)
gtest_add_tests(TARGET test_vio AUTO)
gtest_add_tests(TARGET test_nfr AUTO)
gtest_add_tests(TARGET test_qr AUTO)
gtest_add_tests(TARGET test_linearization AUTO)
gtest_add_tests(TARGET test_patch AUTO)

71
test/ivan-tests/euler2RotTest.cpp Normal file
View File

@@ -0,0 +1,71 @@
//
// Created by ivan on 28.03.2022.
//
#include <iostream>
#include <cmath>
#include <Eigen/Dense>
void euler2Rot(Eigen::Matrix<double, 3, 3>& Rot, double const rx, double const ry, double const rz){
Eigen::Matrix<double, 3, 3> Rx = Eigen::Matrix<double, 3, 3>::Zero();
Eigen::Matrix<double, 3, 3> Ry = Eigen::Matrix<double, 3, 3>::Zero();
Eigen::Matrix<double, 3, 3> Rz = Eigen::Matrix<double, 3, 3>::Zero();
Rx(0, 0) = 1;
Rx(1, 1) = cos(rx);
Rx(2, 1) = sin(rx);
Rx(1, 2) = -sin(rx);
Rx(2, 2) = cos(rx);
std::cout << Rx << std::endl;
std::cout << " " << std::endl;
Ry(0, 0) = cos(ry);
Ry(0, 2) = sin(ry);
Ry(1, 1) = 1;
Ry(2, 0) = -sin(ry);
Ry(2, 2) = cos(ry);
std::cout << Ry << std::endl;
std::cout << " " << std::endl;
Rz(0, 0) = cos(rz);
Rz(0, 1) = -sin(rz);
Rz(1, 0) = sin(rz);
Rz(1, 1) = cos(rz);
Rz(2, 2) = 1;
std::cout << Rz << std::endl;
std::cout << " " << std::endl;
auto Rxy = Rx * Ry;
Rot = Rxy * Rz;
}
int main(int argc, char** argv){
double rx;
double ry;
double rz;
rx = 1.1;
ry = 1.2;
rz = 1.1;
Eigen::Matrix<double, 3, 3> Rot;
euler2Rot(Rot, rx, ry, rz);
std::cout << Rot << std::endl;
std::cout << "" << std::endl;
std::cout << "The eigen matrix multiplication" << std::endl;
Eigen::Matrix<int, 3, 3> M1;
Eigen::Matrix<int, 3, 3> M2;
M1 << 1, 2, 3,
4, 5, 6,
7, 8, 9;
M2 << 1, 2, 3,
4, 5, 6,
7, 8, 9;
std::cout << M1 * M2 << std::endl;
}

418
test/src/test_linearization.cpp Normal file
View File

@@ -0,0 +1,418 @@
#include <basalt/linearization/linearization_base.hpp>
#include <iostream>
#include "gtest/gtest.h"
#include "test_utils.h"
template <class Scalar>
void get_vo_estimator(int num_frames,
basalt::BundleAdjustmentBase<Scalar>& estimator,
basalt::AbsOrderMap& aom) {
static constexpr int POSE_SIZE = 6;
estimator.huber_thresh = 0.5;
estimator.obs_std_dev = 2.0;
estimator.calib.T_i_c.emplace_back(
Sophus::se3_expd(Sophus::Vector6d::Random() / 100));
estimator.calib.T_i_c.emplace_back(
Sophus::se3_expd(Sophus::Vector6d::Random() / 100));
basalt::GenericCamera<Scalar> cam;
cam.variant = basalt::KannalaBrandtCamera4<Scalar>::getTestProjections()[0];
estimator.calib.intrinsics.emplace_back(cam);
estimator.calib.intrinsics.emplace_back(cam);
Eigen::MatrixXd points_3d;
points_3d.setRandom(3, num_frames * 10);
points_3d.row(2).array() += 5.0;
aom.total_size = 0;
for (int i = 0; i < num_frames; i++) {
Sophus::SE3d T_w_i;
T_w_i.so3() = Sophus::SO3d::exp(Eigen::Vector3d::Random() / 100);
T_w_i.translation()[0] = i * 0.1;
aom.abs_order_map[i] = std::make_pair(i * POSE_SIZE, POSE_SIZE);
aom.total_size += POSE_SIZE;
estimator.frame_poses[i] = basalt::PoseStateWithLin(i, T_w_i, false);
for (int j = 0; j < 10; j++) {
const int kp_idx = 10 * i + j;
Eigen::Vector3d p3d = points_3d.col(kp_idx);
basalt::Keypoint<Scalar> kpt;
Sophus::SE3d T_c_w = estimator.calib.T_i_c[0].inverse() * T_w_i.inverse();
Eigen::Vector3d p3d_cam = T_c_w * p3d;
kpt.direction =
basalt::StereographicParam<Scalar>::project(p3d_cam.homogeneous());
kpt.inv_dist = 1.0 / p3d_cam.norm();
kpt.host_kf_id = basalt::TimeCamId(i, 0);
estimator.lmdb.addLandmark(kp_idx, kpt);
for (const auto& [frame_id, frame_pose] : estimator.frame_poses) {
for (int c = 0; c < 2; c++) {
basalt::TimeCamId tcid(frame_id, c);
Sophus::SE3d T_c_w = estimator.calib.T_i_c[c].inverse() *
frame_pose.getPose().inverse();
Eigen::Vector3d p3d_cam = T_c_w * p3d;
Eigen::Vector2d p2d_cam;
cam.project(p3d_cam, p2d_cam);
p2d_cam += Eigen::Vector2d::Random() / 100;
basalt::KeypointObservation<Scalar> ko;
ko.kpt_id = kp_idx;
ko.pos = p2d_cam;
estimator.lmdb.addObservation(tcid, ko);
}
}
}
}
}
template <class Scalar>
void get_vo_estimator_with_marg(int num_frames,
basalt::BundleAdjustmentBase<Scalar>& estimator,
basalt::AbsOrderMap& aom,
basalt::MargLinData<Scalar>& mld) {
static constexpr int POSE_SIZE = 6;
get_vo_estimator(num_frames, estimator, aom);
mld.H.setIdentity(2 * POSE_SIZE, 2 * POSE_SIZE);
mld.H *= 1e6;
mld.b.setRandom(2 * POSE_SIZE);
mld.b *= 10;
mld.order.abs_order_map[0] = std::make_pair(0, POSE_SIZE);
mld.order.abs_order_map[1] = std::make_pair(POSE_SIZE, POSE_SIZE);
mld.order.total_size = 2 * POSE_SIZE;
estimator.frame_poses[0].setLinTrue();
estimator.frame_poses[1].setLinTrue();
estimator.frame_poses[0].applyInc(Sophus::Vector6d::Random() / 100);
estimator.frame_poses[1].applyInc(Sophus::Vector6d::Random() / 100);
}
#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(LinearizationTestSuite, VoNoMargLinearizationTest) {
using Scalar = double;
static constexpr int POSE_SIZE = 6;
static constexpr int NUM_FRAMES = 6;
basalt::BundleAdjustmentBase<Scalar> estimator;
basalt::AbsOrderMap aom;
get_vo_estimator<Scalar>(NUM_FRAMES, estimator, aom);
typename basalt::LinearizationBase<Scalar, POSE_SIZE>::Options options;
options.lb_options.huber_parameter = estimator.huber_thresh;
options.lb_options.obs_std_dev = estimator.obs_std_dev;
Eigen::MatrixXd H_abs_qr, H_abs_sc, H_rel_sc;
Eigen::VectorXd b_abs_qr, b_abs_sc, b_rel_sc;
options.linearization_type = basalt::LinearizationType::ABS_QR;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_qr;
l_abs_qr = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(&estimator,
aom, options);
Scalar error_abs_qr = l_abs_qr->linearizeProblem();
l_abs_qr->performQR();
l_abs_qr->get_dense_H_b(H_abs_qr, b_abs_qr);
options.linearization_type = basalt::LinearizationType::ABS_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_sc;
l_abs_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(&estimator,
aom, options);
Scalar error_abs_sc = l_abs_sc->linearizeProblem();
l_abs_sc->performQR();
l_abs_sc->get_dense_H_b(H_abs_sc, b_abs_sc);
options.linearization_type = basalt::LinearizationType::REL_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_rel_sc;
l_rel_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(&estimator,
aom, options);
Scalar error_rel_sc = l_rel_sc->linearizeProblem();
l_rel_sc->performQR();
l_rel_sc->get_dense_H_b(H_rel_sc, b_rel_sc);
Scalar error_diff = std::abs(error_abs_qr - error_abs_sc);
Scalar H_diff = (H_abs_qr - H_abs_sc).norm();
Scalar b_diff = (b_abs_qr - b_abs_sc).norm();
Scalar error_diff2 = std::abs(error_abs_qr - error_rel_sc);
Scalar H_diff2 = (H_abs_qr - H_rel_sc).norm();
Scalar b_diff2 = (b_abs_qr - b_rel_sc).norm();
EXPECT_LE(error_diff, 1e-8);
EXPECT_LE(H_diff, 1e-8);
EXPECT_LE(b_diff, 1e-8);
EXPECT_LE(error_diff2, 1e-8);
EXPECT_LE(H_diff2, 1e-8);
EXPECT_LE(b_diff2, 1e-8);
}
#endif
#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(LinearizationTestSuite, VoMargLinearizationTest) {
using Scalar = double;
static constexpr int POSE_SIZE = 6;
static constexpr int NUM_FRAMES = 6;
basalt::BundleAdjustmentBase<Scalar> estimator;
basalt::MargLinData<Scalar> mld;
basalt::AbsOrderMap aom;
get_vo_estimator_with_marg<Scalar>(NUM_FRAMES, estimator, aom, mld);
typename basalt::LinearizationBase<Scalar, POSE_SIZE>::Options options;
options.lb_options.huber_parameter = estimator.huber_thresh;
options.lb_options.obs_std_dev = estimator.obs_std_dev;
Eigen::MatrixXd H_abs_qr, H_abs_sc, H_rel_sc;
Eigen::VectorXd b_abs_qr, b_abs_sc, b_rel_sc;
options.linearization_type = basalt::LinearizationType::ABS_QR;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_qr;
l_abs_qr = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator, aom, options, &mld);
Scalar error_abs_qr = l_abs_qr->linearizeProblem();
l_abs_qr->performQR();
l_abs_qr->get_dense_H_b(H_abs_qr, b_abs_qr);
options.linearization_type = basalt::LinearizationType::ABS_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_sc;
l_abs_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator, aom, options, &mld);
Scalar error_abs_sc = l_abs_sc->linearizeProblem();
l_abs_sc->performQR();
l_abs_sc->get_dense_H_b(H_abs_sc, b_abs_sc);
options.linearization_type = basalt::LinearizationType::REL_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_rel_sc;
l_rel_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator, aom, options, &mld);
Scalar error_rel_sc = l_rel_sc->linearizeProblem();
l_rel_sc->performQR();
l_rel_sc->get_dense_H_b(H_rel_sc, b_rel_sc);
Scalar error_diff = std::abs(error_abs_qr - error_abs_sc);
Scalar H_diff = (H_abs_qr - H_abs_sc).norm();
Scalar b_diff = (b_abs_qr - b_abs_sc).norm();
Scalar error_diff2 = std::abs(error_abs_qr - error_rel_sc);
Scalar H_diff2 = (H_abs_qr - H_rel_sc).norm();
Scalar b_diff2 = (b_abs_qr - b_rel_sc).norm();
EXPECT_LE(error_diff, 1e-8);
EXPECT_LE(H_diff, 1e-8);
EXPECT_LE(b_diff, 1e-8);
EXPECT_LE(error_diff2, 1e-8);
EXPECT_LE(H_diff2, 1e-8);
EXPECT_LE(b_diff2, 1e-8);
}
#endif
#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(LinearizationTestSuite, VoMargBacksubstituteTest) {
using Scalar = double;
static constexpr int POSE_SIZE = 6;
static constexpr int NUM_FRAMES = 6;
basalt::BundleAdjustmentBase<Scalar> estimator;
basalt::MargLinData<Scalar> mld;
basalt::AbsOrderMap aom;
get_vo_estimator_with_marg<Scalar>(NUM_FRAMES, estimator, aom, mld);
basalt::BundleAdjustmentBase<Scalar> estimator2 = estimator,
estimator3 = estimator;
typename basalt::LinearizationBase<Scalar, POSE_SIZE>::Options options;
options.lb_options.huber_parameter = estimator.huber_thresh;
options.lb_options.obs_std_dev = estimator.obs_std_dev;
Eigen::MatrixXd H_abs_qr, H_abs_sc, H_rel_sc;
Eigen::VectorXd b_abs_qr, b_abs_sc, b_rel_sc;
options.linearization_type = basalt::LinearizationType::ABS_QR;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_qr;
l_abs_qr = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator, aom, options, &mld);
Scalar error_abs_qr = l_abs_qr->linearizeProblem();
l_abs_qr->performQR();
l_abs_qr->get_dense_H_b(H_abs_qr, b_abs_qr);
options.linearization_type = basalt::LinearizationType::ABS_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_sc;
l_abs_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator2, aom, options, &mld);
Scalar error_abs_sc = l_abs_sc->linearizeProblem();
l_abs_sc->performQR();
l_abs_sc->get_dense_H_b(H_abs_sc, b_abs_sc);
options.linearization_type = basalt::LinearizationType::REL_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_rel_sc;
l_rel_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator3, aom, options, &mld);
Scalar error_rel_sc = l_rel_sc->linearizeProblem();
l_rel_sc->performQR();
l_rel_sc->get_dense_H_b(H_rel_sc, b_rel_sc);
Scalar error_diff = std::abs(error_abs_qr - error_abs_sc);
Scalar H_diff = (H_abs_qr - H_abs_sc).norm();
Scalar b_diff = (b_abs_qr - b_abs_sc).norm();
Scalar error_diff2 = std::abs(error_abs_qr - error_rel_sc);
Scalar H_diff2 = (H_abs_qr - H_rel_sc).norm();
Scalar b_diff2 = (b_abs_qr - b_rel_sc).norm();
EXPECT_LE(error_diff, 1e-8);
EXPECT_LE(H_diff, 1e-8);
EXPECT_LE(b_diff, 1e-8);
EXPECT_LE(error_diff2, 1e-8);
EXPECT_LE(H_diff2, 1e-8);
EXPECT_LE(b_diff2, 1e-8);
const Eigen::VectorXd inc = -H_rel_sc.ldlt().solve(b_rel_sc);
Scalar l_diff1 = l_abs_qr->backSubstitute(inc);
Scalar l_diff2 = l_abs_sc->backSubstitute(inc);
Scalar l_diff3 = l_rel_sc->backSubstitute(inc);
EXPECT_LE(abs(l_diff1 - l_diff2), 1e-8);
EXPECT_LE(abs(l_diff2 - l_diff3), 1e-8);
Scalar error1, error2, error3;
estimator.computeError(error1);
estimator2.computeError(error2);
estimator3.computeError(error3);
EXPECT_LE(abs(error1 - error2), 1e-8);
EXPECT_LE(abs(error1 - error3), 1e-8);
}
#endif
#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(LinearizationTestSuite, VoMargSqrtLinearizationTest) {
using Scalar = double;
static constexpr int POSE_SIZE = 6;
static constexpr int NUM_FRAMES = 6;
basalt::BundleAdjustmentBase<Scalar> estimator;
basalt::MargLinData<Scalar> mld;
basalt::AbsOrderMap aom;
get_vo_estimator_with_marg<Scalar>(NUM_FRAMES, estimator, aom, mld);
typename basalt::LinearizationBase<Scalar, POSE_SIZE>::Options options;
options.lb_options.huber_parameter = estimator.huber_thresh;
options.lb_options.obs_std_dev = estimator.obs_std_dev;
Eigen::MatrixXd H_abs_qr, H_abs_sc, H_rel_sc, Q2TJp_abs_qr, Q2TJp_rel_sc,
Q2TJp_abs_sc;
Eigen::VectorXd b_abs_qr, b_abs_sc, b_rel_sc, Q2Tr_abs_qr, Q2Tr_rel_sc,
Q2Tr_abs_sc;
Scalar error_abs_qr, error_abs_sc, error_rel_sc;
{
options.linearization_type = basalt::LinearizationType::ABS_QR;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_qr;
l_abs_qr = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator, aom, options, &mld);
error_abs_qr = l_abs_qr->linearizeProblem();
l_abs_qr->performQR();
l_abs_qr->get_dense_H_b(H_abs_qr, b_abs_qr);
l_abs_qr->get_dense_Q2Jp_Q2r(Q2TJp_abs_qr, Q2Tr_abs_qr);
}
{
options.linearization_type = basalt::LinearizationType::ABS_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_sc;
l_abs_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator, aom, options, &mld);
error_abs_sc = l_abs_sc->linearizeProblem();
l_abs_sc->performQR();
l_abs_sc->get_dense_H_b(H_abs_sc, b_abs_sc);
l_abs_sc->get_dense_Q2Jp_Q2r(Q2TJp_abs_sc, Q2Tr_abs_sc);
}
{
options.linearization_type = basalt::LinearizationType::REL_SC;
std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_rel_sc;
l_rel_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(
&estimator, aom, options, &mld);
error_rel_sc = l_rel_sc->linearizeProblem();
l_rel_sc->performQR();
l_rel_sc->get_dense_H_b(H_rel_sc, b_rel_sc);
l_rel_sc->get_dense_Q2Jp_Q2r(Q2TJp_rel_sc, Q2Tr_rel_sc);
}
Scalar error_diff = std::abs(error_abs_qr - error_abs_sc);
Scalar H_diff = (H_abs_qr - H_abs_sc).norm();
Scalar b_diff = (b_abs_qr - b_abs_sc).norm();
Scalar error_diff2 = std::abs(error_abs_qr - error_rel_sc);
Scalar H_diff2 = (H_abs_qr - H_rel_sc).norm();
Scalar b_diff2 = (b_abs_qr - b_rel_sc).norm();
EXPECT_LE(error_diff, 1e-8);
EXPECT_LE(H_diff, 1e-8);
EXPECT_LE(b_diff, 1e-8);
EXPECT_LE(error_diff2, 1e-8);
EXPECT_LE(H_diff2, 1e-8);
EXPECT_LE(b_diff2, 1e-8);
// Should hold for full rank problems
Scalar H_diff3 = (Q2TJp_abs_qr.transpose() * Q2TJp_abs_qr - H_abs_qr).norm();
Scalar b_diff3 = (Q2TJp_abs_qr.transpose() * Q2Tr_abs_qr - b_abs_qr).norm();
EXPECT_LE(H_diff3, 1e-3);
EXPECT_LE(b_diff3, 1e-5);
Scalar H_diff4 = (Q2TJp_abs_sc.transpose() * Q2TJp_abs_sc - H_abs_sc).norm();
Scalar b_diff4 = (Q2TJp_abs_sc.transpose() * Q2Tr_abs_sc - b_abs_sc).norm();
EXPECT_LE(H_diff4, 1e-3);
EXPECT_LE(b_diff4, 1e-5);
Scalar H_diff5 = (Q2TJp_rel_sc.transpose() * Q2TJp_rel_sc - H_rel_sc).norm();
Scalar b_diff5 = (Q2TJp_rel_sc.transpose() * Q2Tr_rel_sc - b_rel_sc).norm();
EXPECT_LE(H_diff5, 1e-3);
EXPECT_LE(b_diff5, 1e-5);
}
#endif

137
test/src/test_nfr.cpp Normal file
View File

@@ -0,0 +1,137 @@
#include <basalt/spline/se3_spline.h>
#include <basalt/utils/nfr.h>
#include <iostream>
#include "gtest/gtest.h"
#include "test_utils.h"
static const double accel_std_dev = 0.23;
static const double gyro_std_dev = 0.0027;
// Smaller noise for testing
// static const double accel_std_dev = 0.00023;
// static const double gyro_std_dev = 0.0000027;
std::random_device rd{};
std::mt19937 gen{rd()};
std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev};
std::normal_distribution<> accel_noise_dist{0, accel_std_dev};
TEST(PreIntegrationTestSuite, RelPoseTest) {
Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SE3d T_w_j = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SE3d T_i_j = Sophus::se3_expd(Sophus::Vector6d::Random() / 100) *
T_w_i.inverse() * T_w_j;
Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j;
basalt::relPoseError(T_i_j, T_w_i, T_w_j, &d_res_d_T_w_i, &d_res_d_T_w_j);
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_res_d_T_w_i", d_res_d_T_w_i,
[&](const Sophus::Vector6d& x) {
auto T_w_i_new = T_w_i;
basalt::PoseState<double>::incPose(x, T_w_i_new);
return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j);
},
x0);
}
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_res_d_T_w_j", d_res_d_T_w_j,
[&](const Sophus::Vector6d& x) {
auto T_w_j_new = T_w_j;
basalt::PoseState<double>::incPose(x, T_w_j_new);
return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new);
},
x0);
}
}
TEST(PreIntegrationTestSuite, AbsPositionTest) {
Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random());
Eigen::Vector3d pos = T_w_i.translation() + Eigen::Vector3d::Random() / 10;
Sophus::Matrix<double, 3, 6> d_res_d_T_w_i;
basalt::absPositionError(T_w_i, pos, &d_res_d_T_w_i);
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_res_d_T_w_i", d_res_d_T_w_i,
[&](const Sophus::Vector6d& x) {
auto T_w_i_new = T_w_i;
basalt::PoseState<double>::incPose(x, T_w_i_new);
return basalt::absPositionError(T_w_i_new, pos);
},
x0);
}
}
TEST(PreIntegrationTestSuite, YawTest) {
Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random());
Eigen::Vector3d yaw_dir_body =
T_w_i.so3().inverse() * Eigen::Vector3d::UnitX();
T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random() / 100) * T_w_i;
Sophus::Matrix<double, 1, 6> d_res_d_T_w_i;
basalt::yawError(T_w_i, yaw_dir_body, &d_res_d_T_w_i);
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_res_d_T_w_i", d_res_d_T_w_i,
[&](const Sophus::Vector6d& x) {
auto T_w_i_new = T_w_i;
basalt::PoseState<double>::incPose(x, T_w_i_new);
double res = basalt::yawError(T_w_i_new, yaw_dir_body);
return Eigen::Matrix<double, 1, 1>(res);
},
x0);
}
}
TEST(PreIntegrationTestSuite, RollPitchTest) {
Sophus::SE3d T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SO3d R_w_i = T_w_i.so3();
T_w_i = Sophus::se3_expd(Sophus::Vector6d::Random() / 100) * T_w_i;
Sophus::Matrix<double, 2, 6> d_res_d_T_w_i;
basalt::rollPitchError(T_w_i, R_w_i, &d_res_d_T_w_i);
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_res_d_T_w_i", d_res_d_T_w_i,
[&](const Sophus::Vector6d& x) {
auto T_w_i_new = T_w_i;
basalt::PoseState<double>::incPose(x, T_w_i_new);
return basalt::rollPitchError(T_w_i_new, R_w_i);
},
x0);
}
}

89
test/src/test_patch.cpp Normal file
View File

@@ -0,0 +1,89 @@
#include <basalt/optical_flow/patch.h>
#include <sophus/se2.hpp>
#include <iostream>
#include "gtest/gtest.h"
#include "test_utils.h"
struct SmoothFunction {
template <typename Scalar>
Scalar interp(const Eigen::Matrix<Scalar, 2, 1>& p) const {
return sin(p[0] / 100.0 + p[1] / 20.0);
}
template <typename Scalar>
Eigen::Matrix<Scalar, 3, 1> interpGrad(
const Eigen::Matrix<Scalar, 2, 1>& p) const {
return Eigen::Matrix<Scalar, 3, 1>(sin(p[0] / 100.0 + p[1] / 20.0),
cos(p[0] / 100.0 + p[1] / 20.0) / 100.0,
cos(p[0] / 100.0 + p[1] / 20.0) / 20.0);
}
template <typename Derived>
BASALT_HOST_DEVICE inline bool InBounds(
const Eigen::MatrixBase<Derived>& p,
const typename Derived::Scalar border) const {
return true;
}
};
TEST(Patch, ImageInterpolateGrad) {
Eigen::Vector2i offset(231, 123);
SmoothFunction img;
Eigen::Vector2d pd = offset.cast<double>() + Eigen::Vector2d(0.4, 0.34345);
Eigen::Vector3d val_grad = img.interpGrad<double>(pd);
Eigen::Matrix<double, 1, 2> J_x = val_grad.tail<2>();
test_jacobian(
"d_res_d_x", J_x,
[&](const Eigen::Vector2d& x) {
return Eigen::Matrix<double, 1, 1>(img.interp<double>(pd + x));
},
Eigen::Vector2d::Zero(), 1);
}
TEST(Patch, PatchSe2Jac) {
Eigen::Vector2i offset(231, 123);
SmoothFunction img_view;
Eigen::Vector2d pd = offset.cast<double>() + Eigen::Vector2d(0.4, 0.34345);
using PatternT = basalt::Pattern52<double>;
using PatchT = basalt::OpticalFlowPatch<double, PatternT>;
double mean, mean2;
PatchT::VectorP data, data2;
PatchT::MatrixP3 J_se2;
basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::setDataJacSe2(
img_view, pd, mean, data, J_se2);
basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::setData(
img_view, pd, mean2, data2);
EXPECT_NEAR(mean, mean2, 1e-8);
EXPECT_TRUE(data.isApprox(data2));
test_jacobian(
"d_res_d_se2", J_se2,
[&](const Eigen::Vector3d& x) {
Sophus::SE2d transform = Sophus::SE2d::exp(x);
double mean3;
PatchT::VectorP data3;
basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::setData(
img_view, pd, mean3, data3, &transform);
return data3;
},
Eigen::Vector3d::Zero());
}

127
test/src/test_qr.cpp Normal file
View File

@@ -0,0 +1,127 @@
#include <Eigen/Dense>
#include <iostream>
#include <basalt/vi_estimator/marg_helper.h>
#include "gtest/gtest.h"
TEST(QRTestSuite, QRvsLLT) {
Eigen::MatrixXd J;
J.setRandom(10, 6);
Eigen::HouseholderQR<Eigen::MatrixXd> qr(J);
Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
Eigen::MatrixXd LT = (J.transpose() * J).llt().matrixU();
std::cout << "J\n" << J << "\ncol_norms: " << J.colwise().norm() << std::endl;
std::cout << "R\n" << R << "\ncol_norms: " << R.colwise().norm() << std::endl;
std::cout << "LT\n"
<< LT << "\ncol_norms: " << LT.colwise().norm() << std::endl;
}
TEST(QRTestSuite, QRvsLLTRankDef) {
Eigen::MatrixXd J;
J.setRandom(10, 6);
J.col(2) = J.col(4);
Eigen::HouseholderQR<Eigen::MatrixXd> qr(J);
Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
Eigen::MatrixXd LT = (J.transpose() * J).llt().matrixU();
std::cout << "J\n" << J << "\ncol_norms: " << J.colwise().norm() << std::endl;
std::cout << "R\n" << R << "\ncol_norms: " << R.colwise().norm() << std::endl;
std::cout << "LT\n"
<< LT << "\ncol_norms: " << LT.colwise().norm() << std::endl;
}
#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(QRTestSuite, RankDefLeastSquares) {
Eigen::MatrixXd J;
Eigen::VectorXd r;
J.setRandom(10, 6);
r.setRandom(10);
J.col(1) = J.col(4);
auto decomp = J.completeOrthogonalDecomposition();
Eigen::VectorXd original_solution = decomp.solve(r);
std::cout << "full solution: " << original_solution.transpose() << std::endl;
std::cout << "Rank " << decomp.rank() << std::endl;
std::cout << "sol OR:\t" << original_solution.tail<4>().transpose()
<< std::endl;
std::set<int> idx_to_marg = {0, 1};
std::set<int> idx_to_keep = {2, 3, 4, 5};
Eigen::VectorXd sol_sc, sol_sqrt_sc, sol_sqrt_sc2, sol_qr;
// sqrt SC version
{
Eigen::MatrixXd H = J.transpose() * J;
Eigen::VectorXd b = J.transpose() * r;
Eigen::MatrixXd marg_sqrt_H;
Eigen::VectorXd marg_sqrt_b;
basalt::MargHelper<double>::marginalizeHelperSqToSqrt(
H, b, idx_to_keep, idx_to_marg, marg_sqrt_H, marg_sqrt_b);
auto dec = marg_sqrt_H.completeOrthogonalDecomposition();
sol_sqrt_sc = dec.solve(marg_sqrt_b);
std::cout << "sol SQ:\t" << sol_sqrt_sc.transpose() << std::endl;
std::cout << "rank " << dec.rank() << std::endl;
auto dec2 = (marg_sqrt_H.transpose() * marg_sqrt_H)
.completeOrthogonalDecomposition();
sol_sqrt_sc2 = dec2.solve(marg_sqrt_H.transpose() * marg_sqrt_b);
std::cout << "sol SH:\t" << sol_sqrt_sc2.transpose() << std::endl;
std::cout << "rank " << dec2.rank() << std::endl;
}
// SC version
{
Eigen::MatrixXd H = J.transpose() * J;
Eigen::VectorXd b = J.transpose() * r;
Eigen::MatrixXd marg_H;
Eigen::VectorXd marg_b;
basalt::MargHelper<double>::marginalizeHelperSqToSq(
H, b, idx_to_keep, idx_to_marg, marg_H, marg_b);
auto dec = marg_H.completeOrthogonalDecomposition();
sol_sc = dec.solve(marg_b);
std::cout << "sol SC:\t" << sol_sc.transpose() << std::endl;
std::cout << "rank " << dec.rank() << std::endl;
}
// QR version
{
Eigen::MatrixXd J1 = J;
Eigen::VectorXd r1 = r;
Eigen::MatrixXd marg_sqrt_H;
Eigen::VectorXd marg_sqrt_b;
basalt::MargHelper<double>::marginalizeHelperSqrtToSqrt(
J1, r1, idx_to_keep, idx_to_marg, marg_sqrt_H, marg_sqrt_b);
auto dec = marg_sqrt_H.completeOrthogonalDecomposition();
sol_qr = dec.solve(marg_sqrt_b);
std::cout << "sol QR:\t" << sol_qr.transpose() << std::endl;
std::cout << "rank " << dec.rank() << std::endl;
}
EXPECT_TRUE(sol_qr.isApprox(sol_sc));
EXPECT_TRUE(sol_qr.isApprox(sol_sqrt_sc2));
}
#endif

107
test/src/test_spline_opt.cpp Normal file
View File

@@ -0,0 +1,107 @@
#include <basalt/optimization/spline_optimize.h>
#include <iostream>
#include "gtest/gtest.h"
#include "test_utils.h"
TEST(SplineOpt, SplineOptTest) {
int num_knots = 15;
basalt::CalibAccelBias<double> accel_bias_full;
accel_bias_full.setRandom();
basalt::CalibGyroBias<double> gyro_bias_full;
gyro_bias_full.setRandom();
Eigen::Vector3d g(0, 0, -9.81);
basalt::Se3Spline<5> gt_spline(int64_t(2e9));
gt_spline.genRandomTrajectory(num_knots);
basalt::SplineOptimization<5, double> spline_opt(int64_t(2e9));
int64_t pose_dt_ns = 1e8;
for (int64_t t_ns = pose_dt_ns / 2; t_ns < gt_spline.maxTimeNs();
t_ns += pose_dt_ns) {
Sophus::SE3d pose_gt = gt_spline.pose(t_ns);
spline_opt.addPoseMeasurement(t_ns, pose_gt);
}
int64_t dt_ns = 1e7;
for (int64_t t_ns = 0; t_ns < gt_spline.maxTimeNs(); t_ns += dt_ns) {
Sophus::SE3d pose = gt_spline.pose(t_ns);
Eigen::Vector3d accel_body =
pose.so3().inverse() * (gt_spline.transAccelWorld(t_ns) + g);
// accel_body + accel_bias = (I + scale) * meas
spline_opt.addAccelMeasurement(
t_ns, accel_bias_full.invertCalibration(accel_body));
}
for (int64_t t_ns = 0; t_ns < gt_spline.maxTimeNs(); t_ns += dt_ns) {
Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
spline_opt.addGyroMeasurement(
t_ns, gyro_bias_full.invertCalibration(rot_vel_body));
}
spline_opt.resetCalib(0, {});
spline_opt.initSpline(gt_spline);
spline_opt.setG(g + Eigen::Vector3d::Random() / 10);
spline_opt.init();
double error;
double reprojection_error;
int num_inliers;
for (int i = 0; i < 10; i++)
spline_opt.optimize(false, true, false, false, true, false, 0.002, 1e-10,
error, num_inliers, reprojection_error);
ASSERT_TRUE(
spline_opt.getAccelBias().getParam().isApprox(accel_bias_full.getParam()))
<< "spline_opt.getCalib().accel_bias "
<< spline_opt.getGyroBias().getParam().transpose() << " and accel_bias "
<< accel_bias_full.getParam().transpose() << " are not the same";
ASSERT_TRUE(
spline_opt.getGyroBias().getParam().isApprox(gyro_bias_full.getParam()))
<< "spline_opt.getCalib().gyro_bias "
<< spline_opt.getGyroBias().getParam().transpose() << " and gyro_bias "
<< gyro_bias_full.getParam().transpose() << " are not the same";
ASSERT_TRUE(spline_opt.getG().isApprox(g))
<< "spline_opt.getG() " << spline_opt.getG().transpose() << " and g "
<< g.transpose() << " are not the same";
for (int64_t t_ns = 0; t_ns < gt_spline.maxTimeNs(); t_ns += 1e7) {
Sophus::SE3d pose_gt = gt_spline.pose(t_ns);
Sophus::SE3d pose = spline_opt.getSpline().pose(t_ns);
Eigen::Vector3d pos_gt = pose_gt.translation();
Eigen::Vector3d pos = pose.translation();
Eigen::Quaterniond quat_gt = pose_gt.unit_quaternion();
Eigen::Quaterniond quat = pose.unit_quaternion();
Eigen::Vector3d accel_gt = gt_spline.transAccelWorld(t_ns);
Eigen::Vector3d accel = spline_opt.getSpline().transAccelWorld(t_ns);
Eigen::Vector3d gyro_gt = gt_spline.rotVelBody(t_ns);
Eigen::Vector3d gyro = spline_opt.getSpline().rotVelBody(t_ns);
ASSERT_TRUE(pos_gt.isApprox(pos)) << "pos_gt and pos are not the same";
ASSERT_TRUE(quat_gt.angularDistance(quat) < 1e-2)
<< "quat_gt and quat are not the same";
ASSERT_TRUE(accel_gt.isApprox(accel))
<< "accel_gt and accel are not the same";
ASSERT_TRUE(gyro_gt.isApprox(gyro)) << "gyro_gt and gyro are not the same";
}
}

430
test/src/test_vio.cpp Normal file
View File

@@ -0,0 +1,430 @@
#include <basalt/imu/preintegration.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/utils/ba_utils.h>
#include <basalt/vi_estimator/sc_ba_base.h>
#include <basalt/linearization/imu_block.hpp>
#include <iostream>
#include "gtest/gtest.h"
#include "test_utils.h"
static const double accel_std_dev = 0.23;
static const double gyro_std_dev = 0.0027;
// Smaller noise for testing
// static const double accel_std_dev = 0.00023;
// static const double gyro_std_dev = 0.0000027;
std::random_device rd{};
std::mt19937 gen{rd()};
std::normal_distribution<> gyro_noise_dist{0, gyro_std_dev};
std::normal_distribution<> accel_noise_dist{0, accel_std_dev};
#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(VioTestSuite, ImuNullspace2Test) {
int num_knots = 15;
Eigen::Vector3d bg, ba;
bg = Eigen::Vector3d::Random() / 100;
ba = Eigen::Vector3d::Random() / 10;
basalt::IntegratedImuMeasurement<double> imu_meas(0, bg, ba);
basalt::Se3Spline<5> gt_spline(int64_t(10e9));
gt_spline.genRandomTrajectory(num_knots);
basalt::PoseVelBiasState<double> state0, state1, state1_gt;
state0.t_ns = 0;
state0.T_w_i = gt_spline.pose(int64_t(0));
state0.vel_w_i = gt_spline.transVelWorld(int64_t(0));
state0.bias_gyro = bg;
state0.bias_accel = ba;
Eigen::Vector3d accel_cov, gyro_cov;
accel_cov.setConstant(accel_std_dev * accel_std_dev);
gyro_cov.setConstant(gyro_std_dev * gyro_std_dev);
int64_t dt_ns = 1e7;
for (int64_t t_ns = dt_ns / 2;
t_ns < int64_t(1e8); // gt_spline.maxTimeNs() - int64_t(1e9);
t_ns += dt_ns) {
Sophus::SE3d pose = gt_spline.pose(t_ns);
Eigen::Vector3d accel_body =
pose.so3().inverse() *
(gt_spline.transAccelWorld(t_ns) - basalt::constants::g);
Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
basalt::ImuData<double> data;
data.accel = accel_body + ba;
data.gyro = rot_vel_body + bg;
data.accel[0] += accel_noise_dist(gen);
data.accel[1] += accel_noise_dist(gen);
data.accel[2] += accel_noise_dist(gen);
data.gyro[0] += gyro_noise_dist(gen);
data.gyro[1] += gyro_noise_dist(gen);
data.gyro[2] += gyro_noise_dist(gen);
data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval;
imu_meas.integrate(data, accel_cov, gyro_cov);
}
state1.t_ns = imu_meas.get_dt_ns();
state1.T_w_i = gt_spline.pose(imu_meas.get_dt_ns()) *
Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
state1.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns()) +
Sophus::Vector3d::Random() / 10;
state1.bias_gyro = bg;
state1.bias_accel = ba;
Eigen::Vector3d gyro_weight_sqrt;
gyro_weight_sqrt.setConstant(1e3);
Eigen::Vector3d accel_weight_sqrt;
accel_weight_sqrt.setConstant(1e3);
Eigen::aligned_map<int64_t, basalt::IntegratedImuMeasurement<double>>
imu_meas_vec;
Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin<double>>
frame_states;
Eigen::aligned_map<int64_t, basalt::PoseStateWithLin<double>> frame_poses;
imu_meas_vec[state0.t_ns] = imu_meas;
frame_states[state0.t_ns] = basalt::PoseVelBiasStateWithLin<double>(state0);
frame_states[state1.t_ns] = basalt::PoseVelBiasStateWithLin<double>(state1);
int asize = 30;
basalt::AbsOrderMap aom;
aom.total_size = 30;
aom.items = 2;
aom.abs_order_map[state0.t_ns] = std::make_pair(0, 15);
aom.abs_order_map[state1.t_ns] = std::make_pair(15, 15);
basalt::ImuLinData<double> ild = {basalt::constants::g,
gyro_weight_sqrt,
accel_weight_sqrt,
{std::make_pair(state0.t_ns, &imu_meas)}};
basalt::DenseAccumulator<double> accum;
accum.reset(aom.total_size);
basalt::ImuBlock<double> ib(&imu_meas, &ild, aom);
double e0 = ib.linearizeImu(frame_states);
ib.add_dense_H_b(accum);
// Check quadratic approximation
for (int i = 0; i < 10; i++) {
Eigen::VectorXd rand_inc;
rand_inc.setRandom(asize);
rand_inc.normalize();
rand_inc /= 10000;
auto frame_states_copy = frame_states;
frame_states_copy[state0.t_ns].applyInc(rand_inc.segment<15>(0));
frame_states_copy[state1.t_ns].applyInc(rand_inc.segment<15>(15));
double imu_error_u, bg_error_u, ba_error_u;
basalt::ScBundleAdjustmentBase<double>::computeImuError(
aom, imu_error_u, bg_error_u, ba_error_u, frame_states_copy,
imu_meas_vec, gyro_weight_sqrt.array().square(),
accel_weight_sqrt.array().square(), basalt::constants::g);
double e1 = imu_error_u + bg_error_u + ba_error_u - e0;
double e2 = 0.5 * rand_inc.transpose() * accum.getH() * rand_inc;
e2 += rand_inc.transpose() * accum.getB();
EXPECT_LE(std::abs(e1 - e2), 2e-2) << "e1 " << e1 << " e2 " << e2;
}
std::cout << "=========================================" << std::endl;
Eigen::VectorXd null_res =
basalt::ScBundleAdjustmentBase<double>::checkNullspace(
accum.getH(), accum.getB(), aom, frame_states, frame_poses);
std::cout << "=========================================" << std::endl;
EXPECT_LE(std::abs(null_res[0]), 1e-8);
EXPECT_LE(std::abs(null_res[1]), 1e-8);
EXPECT_LE(std::abs(null_res[2]), 1e-8);
EXPECT_LE(std::abs(null_res[5]), 1e-6);
}
#endif
#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(VioTestSuite, ImuNullspace3Test) {
int num_knots = 15;
Eigen::Vector3d bg, ba;
bg = Eigen::Vector3d::Random() / 100;
ba = Eigen::Vector3d::Random() / 10;
basalt::IntegratedImuMeasurement<double> imu_meas1(0, bg, ba);
basalt::Se3Spline<5> gt_spline(int64_t(10e9));
gt_spline.genRandomTrajectory(num_knots);
basalt::PoseVelBiasState<double> state0, state1, state2;
state0.t_ns = 0;
state0.T_w_i = gt_spline.pose(int64_t(0));
state0.vel_w_i = gt_spline.transVelWorld(int64_t(0));
state0.bias_gyro = bg;
state0.bias_accel = ba;
Eigen::Vector3d accel_cov, gyro_cov;
accel_cov.setConstant(accel_std_dev * accel_std_dev);
gyro_cov.setConstant(gyro_std_dev * gyro_std_dev);
int64_t dt_ns = 1e7;
for (int64_t t_ns = dt_ns / 2;
t_ns < int64_t(1e9); // gt_spline.maxTimeNs() - int64_t(1e9);
t_ns += dt_ns) {
Sophus::SE3d pose = gt_spline.pose(t_ns);
Eigen::Vector3d accel_body =
pose.so3().inverse() *
(gt_spline.transAccelWorld(t_ns) - basalt::constants::g);
Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
basalt::ImuData<double> data;
data.accel = accel_body + ba;
data.gyro = rot_vel_body + bg;
data.accel[0] += accel_noise_dist(gen);
data.accel[1] += accel_noise_dist(gen);
data.accel[2] += accel_noise_dist(gen);
data.gyro[0] += gyro_noise_dist(gen);
data.gyro[1] += gyro_noise_dist(gen);
data.gyro[2] += gyro_noise_dist(gen);
data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval;
imu_meas1.integrate(data, accel_cov, gyro_cov);
}
basalt::IntegratedImuMeasurement<double> imu_meas2(imu_meas1.get_dt_ns(), bg,
ba);
for (int64_t t_ns = imu_meas1.get_dt_ns() + dt_ns / 2;
t_ns < int64_t(2e9); // gt_spline.maxTimeNs() - int64_t(1e9);
t_ns += dt_ns) {
Sophus::SE3d pose = gt_spline.pose(t_ns);
Eigen::Vector3d accel_body =
pose.so3().inverse() *
(gt_spline.transAccelWorld(t_ns) - basalt::constants::g);
Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
basalt::ImuData<double> data;
data.accel = accel_body + ba;
data.gyro = rot_vel_body + bg;
data.accel[0] += accel_noise_dist(gen);
data.accel[1] += accel_noise_dist(gen);
data.accel[2] += accel_noise_dist(gen);
data.gyro[0] += gyro_noise_dist(gen);
data.gyro[1] += gyro_noise_dist(gen);
data.gyro[2] += gyro_noise_dist(gen);
data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval;
imu_meas2.integrate(data, accel_cov, gyro_cov);
}
state1.t_ns = imu_meas1.get_dt_ns();
state1.T_w_i = gt_spline.pose(state1.t_ns) *
Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
state1.vel_w_i =
gt_spline.transVelWorld(state1.t_ns) + Sophus::Vector3d::Random() / 10;
state1.bias_gyro = bg;
state1.bias_accel = ba;
state2.t_ns = imu_meas1.get_dt_ns() + imu_meas2.get_dt_ns();
state2.T_w_i = gt_spline.pose(state2.t_ns) *
Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
state2.vel_w_i =
gt_spline.transVelWorld(state2.t_ns) + Sophus::Vector3d::Random() / 10;
state2.bias_gyro = bg;
state2.bias_accel = ba;
Eigen::Vector3d gyro_weight_sqrt;
gyro_weight_sqrt.setConstant(1e3);
Eigen::Vector3d accel_weight_sqrt;
accel_weight_sqrt.setConstant(1e3);
Eigen::aligned_map<int64_t, basalt::IntegratedImuMeasurement<double>>
imu_meas_vec;
Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin<double>>
frame_states;
Eigen::aligned_map<int64_t, basalt::PoseStateWithLin<double>> frame_poses;
imu_meas_vec[imu_meas1.get_start_t_ns()] = imu_meas1;
imu_meas_vec[imu_meas2.get_start_t_ns()] = imu_meas2;
frame_states[state0.t_ns] = basalt::PoseVelBiasStateWithLin<double>(state0);
frame_states[state1.t_ns] = basalt::PoseVelBiasStateWithLin<double>(state1);
frame_states[state2.t_ns] = basalt::PoseVelBiasStateWithLin<double>(state2);
int asize = 45;
basalt::AbsOrderMap aom;
aom.total_size = asize;
aom.items = 2;
aom.abs_order_map[state0.t_ns] = std::make_pair(0, 15);
aom.abs_order_map[state1.t_ns] = std::make_pair(15, 15);
aom.abs_order_map[state2.t_ns] = std::make_pair(30, 15);
basalt::ImuLinData<double> ild = {basalt::constants::g,
gyro_weight_sqrt,
accel_weight_sqrt,
{
std::make_pair(state0.t_ns, &imu_meas1),
std::make_pair(state1.t_ns, &imu_meas2),
}};
basalt::DenseAccumulator<double> accum;
accum.reset(aom.total_size);
basalt::ImuBlock<double> ib1(&imu_meas1, &ild, aom),
ib2(&imu_meas2, &ild, aom);
ib1.linearizeImu(frame_states);
ib2.linearizeImu(frame_states);
ib1.add_dense_H_b(accum);
ib2.add_dense_H_b(accum);
std::cout << "=========================================" << std::endl;
Eigen::VectorXd null_res =
basalt::ScBundleAdjustmentBase<double>::checkNullspace(
accum.getH(), accum.getB(), aom, frame_states, frame_poses);
std::cout << "=========================================" << std::endl;
EXPECT_LE(std::abs(null_res[0]), 1e-8);
EXPECT_LE(std::abs(null_res[1]), 1e-8);
EXPECT_LE(std::abs(null_res[2]), 1e-8);
EXPECT_LE(std::abs(null_res[5]), 1e-6);
}
#endif
TEST(VioTestSuite, RelPoseTest) {
Sophus::SE3d T_w_i_h = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SE3d T_w_i_t = Sophus::se3_expd(Sophus::Vector6d::Random());
Sophus::SE3d T_i_c_h = Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
Sophus::SE3d T_i_c_t = Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
Sophus::Matrix6d d_rel_d_h, d_rel_d_t;
Sophus::SE3d T_t_h_sophus = basalt::computeRelPose(
T_w_i_h, T_i_c_h, T_w_i_t, T_i_c_t, &d_rel_d_h, &d_rel_d_t);
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_rel_d_h", d_rel_d_h,
[&](const Sophus::Vector6d& x) {
Sophus::SE3d T_w_h_new = T_w_i_h;
basalt::PoseState<double>::incPose(x, T_w_h_new);
Sophus::SE3d T_t_h_sophus_new =
basalt::computeRelPose(T_w_h_new, T_i_c_h, T_w_i_t, T_i_c_t);
return Sophus::se3_logd(T_t_h_sophus_new * T_t_h_sophus.inverse());
},
x0);
}
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_rel_d_t", d_rel_d_t,
[&](const Sophus::Vector6d& x) {
Sophus::SE3d T_w_t_new = T_w_i_t;
basalt::PoseState<double>::incPose(x, T_w_t_new);
Sophus::SE3d T_t_h_sophus_new =
basalt::computeRelPose(T_w_i_h, T_i_c_h, T_w_t_new, T_i_c_t);
return Sophus::se3_logd(T_t_h_sophus_new * T_t_h_sophus.inverse());
},
x0);
}
}
TEST(VioTestSuite, LinearizePointsTest) {
basalt::ExtendedUnifiedCamera<double> cam =
basalt::ExtendedUnifiedCamera<double>::getTestProjections()[0];
basalt::Keypoint<double> kpt_pos;
Eigen::Vector4d point3d;
cam.unproject(Eigen::Vector2d::Random() * 50, point3d);
kpt_pos.direction = basalt::StereographicParam<double>::project(point3d);
kpt_pos.inv_dist = 0.1231231;
Sophus::SE3d T_w_h = Sophus::se3_expd(Sophus::Vector6d::Random() / 100);
Sophus::SE3d T_w_t = Sophus::se3_expd(Sophus::Vector6d::Random() / 100);
T_w_t.translation()[0] += 0.1;
Sophus::SE3d T_t_h_sophus = T_w_t.inverse() * T_w_h;
Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix();
Eigen::Vector4d p_trans;
p_trans = basalt::StereographicParam<double>::unproject(kpt_pos.direction);
p_trans(3) = kpt_pos.inv_dist;
p_trans = T_t_h * p_trans;
basalt::KeypointObservation<double> kpt_obs;
cam.project(p_trans, kpt_obs.pos);
Eigen::Vector2d res;
Eigen::Matrix<double, 2, 6> d_res_d_xi;
Eigen::Matrix<double, 2, 3> d_res_d_p;
basalt::linearizePoint(kpt_obs.pos, kpt_pos, T_t_h, cam, res, &d_res_d_xi,
&d_res_d_p);
{
Sophus::Vector6d x0;
x0.setZero();
test_jacobian(
"d_res_d_xi", d_res_d_xi,
[&](const Sophus::Vector6d& x) {
Eigen::Matrix4d T_t_h_new =
(Sophus::se3_expd(x) * T_t_h_sophus).matrix();
Eigen::Vector2d res;
basalt::linearizePoint(kpt_obs.pos, kpt_pos, T_t_h_new, cam, res);
return res;
},
x0);
}
{
Eigen::Vector3d x0;
x0.setZero();
test_jacobian(
"d_res_d_p", d_res_d_p,
[&](const Eigen::Vector3d& x) {
basalt::Keypoint kpt_pos_new = kpt_pos;
kpt_pos_new.direction += x.head<2>();
kpt_pos_new.inv_dist += x[2];
Eigen::Vector2d res;
basalt::linearizePoint(kpt_obs.pos, kpt_pos_new, T_t_h, cam, res);
return res;
},
x0);
}
}