diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 812ee1f8e4233825bb78aae2029cbd349c34afc1..6fa50395a7ee2af50d290b32e543b18bac534869 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -172,7 +172,6 @@ wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp) # FactorRelativePose3dWithExtrinsics class test wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp) -wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics_new gtest_factor_relative_pose_3d_with_extrinsics_new.cpp) # FactorVelocityLocalDirection3d class test wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp) diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp index f4755e11139f07ba42d537e0fc4e070c300cf9a3..54f4fc527a9d470442b2a1d6352ef064af6c21d4 100644 --- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp @@ -1,326 +1,378 @@ -//--------LICENSE_START-------- -// -// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. -// Authors: Joan Solà Ortega (jsola@iri.upc.edu) -// All rights reserved. -// -// This file is part of WOLF -// WOLF is free software: you can redistribute it and/or modify -// it under the terms of the GNU Lesser General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU Lesser General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public License -// along with this program. If not, see <http://www.gnu.org/licenses/>. -// -//--------LICENSE_END-------- -#include <core/factor/factor_relative_pose_3d_with_extrinsics.h> -#include <core/utils/utils_gtest.h> - -#include "core/common/wolf.h" -#include "core/utils/logging.h" - -#include "core/state_block/state_block_derived.h" -#include "core/state_block/state_quaternion.h" -#include "core/ceres_wrapper/solver_ceres.h" -#include "core/capture/capture_pose.h" -#include "core/feature/feature_pose.h" - - - -using namespace Eigen; -using namespace wolf; -using std::static_pointer_cast; - - - -// Use the following in case you want to initialize tests with predefines variables or methods. -class FactorRelativePose3dWithExtrinsics_class : public testing::Test{ - public: - Vector3d pos_camera, pos_robot, pos_landmark; - Vector3d euler_camera, euler_robot, euler_landmark; - Quaterniond quat_camera, quat_robot, quat_landmark; - Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors - Vector7d pose_camera, pose_robot, pose_landmark; - - ProblemPtr problem; - SolverCeresPtr solver; - - SensorBasePtr S; - FrameBasePtr F1; - CapturePosePtr c1; - FeaturePosePtr f1; - LandmarkBasePtr lmk1; - FactorRelativePose3dWithExtrinsicsPtr fac; - - Eigen::Matrix6d meas_cov; - - void SetUp() override + //--------LICENSE_START-------- + // + // Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. + // Authors: Joan Solà Ortega (jsola@iri.upc.edu) + // All rights reserved. + // + // This file is part of WOLF + // WOLF is free software: you can redistribute it and/or modify + // it under the terms of the GNU Lesser General Public License as published by + // the Free Software Foundation, either version 3 of the License, or + // at your option) any later version. + // + // This program is distributed in the hope that it will be useful, + // but WITHOUT ANY WARRANTY; without even the implied warranty of + // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + // GNU Lesser General Public License for more details. + // + // You should have received a copy of the GNU Lesser General Public License + // along with this program. If not, see <http://www.gnu.org/licenses/>. + // + //--------LICENSE_END-------- + #include "core/utils/utils_gtest.h" + + #include "core/ceres_wrapper/solver_ceres.h" + #include "core/factor/factor_relative_pose_3d_with_extrinsics.h" + #include "core/capture/capture_odom_3d.h" + #include "core/sensor/sensor_odom_3d.h" + #include "core/math/rotations.h" + #include "core/state_block/state_block_derived.h" + #include "core/state_block/state_quaternion.h" + + + using namespace Eigen; + using namespace wolf; + using std::cout; + using std::endl; + + std::string wolf_root = _WOLF_ROOT_DIR; + + int N = 1e2; + + // Vectors + Vector7d delta, x_0, x_1, x_f, x_l, x_s; + + // Input odometry data and covariance + Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal(); + + // Problem and solver + ProblemPtr problem_ptr = Problem::create("PO", 3); + + SolverCeres solver(problem_ptr); + + // Sensor + auto sensor_odom3d = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml"); + + // Two frames + FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() ); + FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() ); + + // Landmark + LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), + "LandmarkPose3d", + std::make_shared<StatePoint3d>(Vector3d::Zero()), + std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs())); + + // Capture + auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor_odom3d, Vector7d::Zero(), data_cov); + // Feature + FeatureBasePtr fea = nullptr; + // Factor + FactorRelativePose3dWithExtrinsicsPtr fac = nullptr; + + void generateRandomProblemFrame() + { + // Random delta + delta = Vector7d::Random() * 1e2; + delta.tail<4>().normalize(); + auto q_delta = Quaterniond(delta.tail<4>()); + + // Random frame 0 pose + x_0 = Vector7d::Random() * 1e2; + x_0.tail<4>().normalize(); + auto q_0 = Quaterniond(x_0.tail<4>()); + + // Random extrinsics + x_s = Vector7d::Random() * 1e2; + x_s.tail<4>().normalize(); + auto q_s = Quaterniond(x_s.tail<4>()); + + // Random frame 1 pose + auto q_1 = q_0 * q_s * q_delta * q_s.conjugate(); + x_1.head<3>() = x_0.head<3>() + q_0 * (x_s.head<3>() + q_s * delta.head<3>()) - q_1 * x_s.head<3>(); + x_1.tail<4>() = q_1.coeffs(); + + // WOLF_INFO("x_0: ", x_0.transpose()); + // WOLF_INFO("x_s: ", x_s.transpose()); + // WOLF_INFO("delta: ", delta.transpose()); + // WOLF_INFO("x_1: ", x_1.transpose()); + + // Set states + frm0->setState(x_0); + frm1->setState(x_1); + cap1->setData(delta); + sensor_odom3d->setState(x_s); + + // feature & factor with delta measurement + fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov); + fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add + } + + void generateRandomProblemLandmark() + { + // Random delta + delta = Vector7d::Random() * 10; + delta.tail<4>().normalize(); + auto q_delta = Quaterniond(delta.tail<4>()); + + // Random frame pose + x_f = Vector7d::Random() * 10; + x_f.tail<4>().normalize(); + auto q_f = Quaterniond(x_f.tail<4>()); + + // Random extrinsics + x_s = Vector7d::Random() * 10; + x_s.tail<4>().normalize(); + auto q_s = Quaterniond(x_s.tail<4>()); + + // landmark pose + x_l.head<3>() = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta.head<3>()); + x_l.tail<4>() = (q_f * q_s * q_delta).coeffs(); + + // Set states + frm1->setState(x_f); + lmk->setState(x_l); + cap1->setData(delta); + sensor_odom3d->setState(x_s); + + // feature & factor with delta measurement + fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov); + fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add + } + + TEST(FactorRelativePose3dWithExtrinsics, check_tree) + { + ASSERT_TRUE(problem_ptr->check(0)); + } + + // FRAME TO FRAME ========================================================================= + TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1) + { + for (int i = 0; i < N; i++) { - // configuration - - /* We have three poses to take into account: - * - pose of the sensor (extrinsincs) - * - pose of the landmark - * - pose of the robot (Keyframe) - * - * The measurement provides the pose of the landmark wrt sensor's current pose in the world. - * Camera's current pose in World is the composition of the robot pose with the sensor extrinsics. - * - * The robot has a sensor looking forward - * S: pos = (0,0,0), ori = (0, 0, 0) - * - * There is a point at the origin - * P: pos = (0,0,0) - * - * Therefore, P projects exactly at the origin of the sensor, - * f: p = (0,0) - * - * We form a Wolf tree with 1 frames F1, 1 capture C1, - * 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1: - * - * Frame F1, Capture C1, feature f1, landmark l1, constraint c1 - * - * The frame pose F1, and the sensor pose S - * in the robot frame are variables subject to optimization - * - * We perform a number of tests based on this configuration.*/ - - - // sensor is at origin of robot and looking forward - // robot is at (0,0,0) - // landmark is right in front of sensor. Its orientation wrt sensor is identity. - pos_camera << 0,0,0; - pos_robot << 0,0,0; //robot is at origin - pos_landmark << 0,1,0; - euler_camera << 0,0,0; - //euler_camera << -M_PI_2, 0, -M_PI_2; - euler_robot << 0,0,0; - euler_landmark << 0,0,0; - quat_camera = e2q(euler_camera); - quat_robot = e2q(euler_robot); - quat_landmark = e2q(euler_landmark); - vquat_camera = quat_camera.coeffs(); - vquat_robot = quat_robot.coeffs(); - vquat_landmark = quat_landmark.coeffs(); - pose_camera << pos_camera, vquat_camera; - pose_robot << pos_robot, vquat_robot; - pose_landmark << pos_landmark, vquat_landmark; - - // Build problem - problem = Problem::create("PO", 3); - solver = std::make_shared<SolverCeres>(problem); - - // Create sensor to be able to initialize (a camera for instance) - S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose", - std::make_shared<StatePoint3d>(pos_camera, true), - std::make_shared<StateQuaternion>(vquat_camera, true), - std::make_shared<StateParams4>(Vector4d::Zero(), false), // fixed - Vector1d::Zero()); - - // ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>(); - // S = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera); - // camera = std::static_pointer_cast<SensorCamera>(S); - - - // F1 is be origin KF - VectorComposite x0(pose_robot, "PO", {3,4}); - VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)}); - F1 = problem->setPriorFactor(x0, s0, 0.0); - - - // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world) - meas_cov = Eigen::Matrix6d::Identity(); - meas_cov.topLeftCorner(3,3) *= 1e-2; - meas_cov.bottomRightCorner(3,3) *= 1e-3; - - //emplace feature and landmark - c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov)); - f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov)); - lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose", - std::make_shared<StatePoint3d>(pos_landmark), - std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark))); + // random setup + generateRandomProblemFrame(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb frm1, fix the rest + frm0->fix(); + frm1->unfix(); + sensor_odom3d->getP()->fix(); + sensor_odom3d->getO()->fix(); + frm1->perturb(1); + + // solve for frm1 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); + } + } + + TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0) + { + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemFrame(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb frm0, fix the rest + frm1->fix(); + frm0->unfix(); + sensor_odom3d->getP()->fix(); + sensor_odom3d->getO()->fix(); + frm0->perturb(1); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); + } + } + + // JV: The position extrinsics in case of pair of frames apears to be not very observable + TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p) + { + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemFrame(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb sensor P, fix the rest + frm1->fix(); + frm0->fix(); + sensor_odom3d->getP()->unfix(); + sensor_odom3d->getO()->fix(); + //WOLF_INFO("sensor P before perturbing: ", sensor_odom3d->getP()->getState().transpose()); + sensor_odom3d->getP()->perturb(1); + //WOLF_INFO("sensor P after perturbing: ", sensor_odom3d->getP()->getState().transpose()); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO("sensor P after solving: ", sensor_odom3d->getP()->getState().transpose()); + //WOLF_INFO(report); + + //ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); + } + } + + TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o) + { + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemFrame(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb sensor O, fix the rest + frm1->fix(); + frm0->fix(); + sensor_odom3d->getP()->fix(); + sensor_odom3d->getO()->unfix(); + sensor_odom3d->getO()->perturb(.2); + + //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl; + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); } -}; - - -TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor) -{ - auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1, - lmk1, - nullptr, - false); - - ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics"); -} - -///////////////////////////////////////////// -// Tree not ok with this gtest implementation -///////////////////////////////////////////// -TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree) -{ - ASSERT_TRUE(problem->check(1)); - - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, - f1, - lmk1, - nullptr, - false); - ASSERT_TRUE(problem->check(1)); -} - -TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated) -{ - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, - f1, - lmk1, - nullptr, - false); - - // unfix F1, perturbate state - F1->unfix(); - F1->getP()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); -} - -TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated) -{ - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, - f1, - lmk1, - nullptr, - false); - - // unfix F1, perturbate state - F1->unfix(); - F1->getO()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); -} - -TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization) -{ - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, - f1, - lmk1, - nullptr, - false); - - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); - -} - -TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated) -{ - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, - f1, - lmk1, - nullptr, - false); - - // unfix lmk1, perturbate state - lmk1->unfix(); - lmk1->getP()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); -} - -TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated) -{ - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, - f1, - lmk1, - nullptr, - false); - - // unfix F1, perturbate state - lmk1->unfix(); - lmk1->getO()->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); - -} - -TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated) -{ - // Change setup - Vector3d p_w_r, p_r_c, p_c_l, p_w_l; - Quaterniond q_w_r, q_r_c, q_c_l, q_w_l; - p_w_r << 1, 2, 3; - p_r_c << 4, 5, 6; - p_c_l << 7, 8, 9; - q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize(); - q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize(); - q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize(); - - q_w_l = q_w_r * q_r_c * q_c_l; - p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l); - - // Change feature (remove and emplace) - Vector7d meas; - meas << p_c_l, q_c_l.coeffs(); - f1->remove(); - f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov)); - - // emplace factor - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, - f1, - lmk1, - nullptr, - false); - - // Change Landmark - lmk1->getP()->setState(p_w_l); - lmk1->getO()->setState(q_w_l.coeffs()); - ASSERT_TRUE(lmk1->getP()->stateUpdated()); - ASSERT_TRUE(lmk1->getO()->stateUpdated()); - - // Change Frame - F1->getP()->setState(p_w_r); - F1->getO()->setState(q_w_r.coeffs()); - F1->fix(); - ASSERT_TRUE(F1->getP()->stateUpdated()); - ASSERT_TRUE(F1->getO()->stateUpdated()); - - // Change sensor extrinsics - S->getP()->setState(p_r_c); - S->getO()->setState(q_r_c.coeffs()); - ASSERT_TRUE(S->getP()->stateUpdated()); - ASSERT_TRUE(S->getO()->stateUpdated()); - - Vector7d t_w_r, t_w_l; - t_w_r << p_w_r, q_w_r.coeffs(); - t_w_l << p_w_l, q_w_l.coeffs(); - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6); - - // unfix LMK, perturbate state - lmk1->unfix(); - lmk1->perturb(); - - std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6); - -} - -int main(int argc, char **argv) -{ + } + + // FRAME TO LANDMARK ========================================================================= + TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk) + { + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb landmark, fix the rest + frm1->fix(); + lmk->unfix(); + sensor_odom3d->getP()->fix(); + sensor_odom3d->getO()->fix(); + lmk->perturb(1); + + // solve for landmark + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); + } + } + + TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame) + { + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb frm0, fix the rest + frm1->unfix(); + lmk->fix(); + sensor_odom3d->getP()->fix(); + sensor_odom3d->getO()->fix(); + frm1->perturb(1); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); + } + } + + TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve) + { + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb sensor P, fix the rest + frm1->fix(); + lmk->fix(); + sensor_odom3d->getP()->unfix(); + sensor_odom3d->getO()->fix(); + sensor_odom3d->getP()->perturb(1); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); + } + } + + TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve) + { + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); + + // Perturb sensor O, fix the rest + frm1->fix(); + lmk->fix(); + sensor_odom3d->getP()->fix(); + sensor_odom3d->getO()->unfix(); + sensor_odom3d->getO()->perturb(.2); + + //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl; + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3); + + // remove feature (and factor) for the next loop + fea->remove(); + } + } + + int main(int argc, char **argv) + { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} - + } diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp deleted file mode 100644 index 7b83aa4a8d014415fbd80405c47197e6eddaaba5..0000000000000000000000000000000000000000 --- a/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp +++ /dev/null @@ -1,377 +0,0 @@ - //--------LICENSE_START-------- - // - // Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. - // Authors: Joan Solà Ortega (jsola@iri.upc.edu) - // All rights reserved. - // - // This file is part of WOLF - // WOLF is free software: you can redistribute it and/or modify - // it under the terms of the GNU Lesser General Public License as published by - // the Free Software Foundation, either version 3 of the License, or - // at your option) any later version. - // - // This program is distributed in the hope that it will be useful, - // but WITHOUT ANY WARRANTY; without even the implied warranty of - // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - // GNU Lesser General Public License for more details. - // - // You should have received a copy of the GNU Lesser General Public License - // along with this program. If not, see <http://www.gnu.org/licenses/>. - // - //--------LICENSE_END-------- - #include "core/utils/utils_gtest.h" - - #include "core/ceres_wrapper/solver_ceres.h" - #include "core/factor/factor_relative_pose_3d_with_extrinsics.h" - #include "core/capture/capture_odom_3d.h" - #include "core/sensor/sensor_odom_3d.h" - #include "core/math/rotations.h" - #include "core/state_block/state_block_derived.h" - #include "core/state_block/state_quaternion.h" - - - using namespace Eigen; - using namespace wolf; - using std::cout; - using std::endl; - - std::string wolf_root = _WOLF_ROOT_DIR; - - int N = 1;//1e2 - - // Vectors - Vector7d delta, x_0, x_1, x_f, x_l, x_s; - - // Input odometry data and covariance - Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal(); - - // Problem and solver - ProblemPtr problem_ptr = Problem::create("PO", 3); - - SolverCeres solver(problem_ptr); - - // Sensor - auto sensor_odom3d = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml"); - - // Two frames - FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() ); - FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() ); - - // Landmark - LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), - "LandmarkPose3d", - std::make_shared<StatePoint3d>(Vector3d::Zero()), - std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs())); - - // Capture - auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor_odom3d, Vector7d::Zero(), data_cov); - // Feature - FeatureBasePtr fea = nullptr; - // Factor - FactorRelativePose3dWithExtrinsicsPtr fac = nullptr; - - void generateRandomProblemFrame() - { - // Random delta - delta = Vector7d::Random() * 1e2; - delta.tail<4>().normalize(); - auto q_delta = Quaterniond(delta.tail<4>()); - - // Random frame 0 pose - x_0 = Vector7d::Random() * 1e2; - x_0.tail<4>().normalize(); - auto q_0 = Quaterniond(x_0.tail<4>()); - - // Random extrinsics - x_s = Vector7d::Random() * 1e2; - x_s.tail<4>().normalize(); - auto q_s = Quaterniond(x_s.tail<4>()); - - // Random frame 1 pose - auto q_1 = q_0 * q_s * q_delta * q_s.conjugate(); - x_1.head<3>() = x_0.head<3>() + q_0 * (x_s.head<3>() + q_s * delta.head<3>()) - q_1 * x_s.head<3>(); - x_1.tail<4>() = q_1.coeffs(); - - WOLF_INFO("x_0: ", x_0.transpose()); - WOLF_INFO("x_s: ", x_s.transpose()); - WOLF_INFO("delta: ", delta.transpose()); - WOLF_INFO("x_1: ", x_1.transpose()); - - // Set states - frm0->setState(x_0); - frm1->setState(x_1); - cap1->setData(delta); - sensor_odom3d->setState(x_s); - - // feature & factor with delta measurement - fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov); - fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add - } - - void generateRandomProblemLandmark() - { - // Random delta - delta = Vector7d::Random() * 10; - delta.tail<4>().normalize(); - auto q_delta = Quaterniond(delta.tail<4>()); - - // Random frame pose - x_f = Vector7d::Random() * 10; - x_f.tail<4>().normalize(); - auto q_f = Quaterniond(x_f.tail<4>()); - - // Random extrinsics - x_s = Vector7d::Random() * 10; - x_s.tail<4>().normalize(); - auto q_s = Quaterniond(x_s.tail<4>()); - - // landmark pose - x_l.head<3>() = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta.head<3>()); - x_l.tail<4>() = (q_f * q_s * q_delta).coeffs(); - - // Set states - frm1->setState(x_f); - lmk->setState(x_l); - cap1->setData(delta); - sensor_odom3d->setState(x_s); - - // feature & factor with delta measurement - fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov); - fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add - } - - TEST(FactorRelativePose3dWithExtrinsics, check_tree) - { - ASSERT_TRUE(problem_ptr->check(0)); - } - - // FRAME TO FRAME ========================================================================= - TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemFrame(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb frm1, fix the rest - frm0->fix(); - frm1->unfix(); - sensor_odom3d->getP()->fix(); - sensor_odom3d->getO()->fix(); - frm1->perturb(1); - - // solve for frm1 - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemFrame(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb frm0, fix the rest - frm1->fix(); - frm0->unfix(); - sensor_odom3d->getP()->fix(); - sensor_odom3d->getO()->fix(); - frm0->perturb(1); - - // solve for frm0 - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemFrame(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb sensor P, fix the rest - frm1->fix(); - frm0->fix(); - sensor_odom3d->getP()->unfix(); - sensor_odom3d->getO()->fix(); - WOLF_INFO("sensor P before perturbing: ", sensor_odom3d->getP()->getState().transpose()); - sensor_odom3d->getP()->perturb(1); - WOLF_INFO("sensor P after perturbing: ", sensor_odom3d->getP()->getState().transpose()); - - // solve for frm0 - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO("sensor P after solving: ", sensor_odom3d->getP()->getState().transpose()); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemFrame(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb sensor O, fix the rest - frm1->fix(); - frm0->fix(); - sensor_odom3d->getP()->fix(); - sensor_odom3d->getO()->unfix(); - sensor_odom3d->getO()->perturb(.2); - - //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl; - - // solve for frm0 - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - // FRAME TO LANDMARK ========================================================================= - TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemLandmark(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb landmark, fix the rest - frm1->fix(); - lmk->unfix(); - sensor_odom3d->getP()->fix(); - sensor_odom3d->getO()->fix(); - lmk->perturb(1); - - // solve for landmark - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemLandmark(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb frm0, fix the rest - frm1->unfix(); - lmk->fix(); - sensor_odom3d->getP()->fix(); - sensor_odom3d->getO()->fix(); - frm1->perturb(1); - - // solve for frm0 - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemLandmark(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb sensor P, fix the rest - frm1->fix(); - lmk->fix(); - sensor_odom3d->getP()->unfix(); - sensor_odom3d->getO()->fix(); - sensor_odom3d->getP()->perturb(1); - - // solve for frm0 - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve) - { - for (int i = 0; i < N; i++) - { - // random setup - generateRandomProblemLandmark(); - - ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS); - - // Perturb sensor O, fix the rest - frm1->fix(); - lmk->fix(); - sensor_odom3d->getP()->fix(); - sensor_odom3d->getO()->unfix(); - sensor_odom3d->getO()->perturb(.2); - - //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl; - - // solve for frm0 - std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); - WOLF_INFO(report); - - ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6); - - // remove feature (and factor) for the next loop - fea->remove(); - } - } - - int main(int argc, char **argv) - { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); - }