Skip to content
Snippets Groups Projects
Commit c4f3802e authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

fixed factor_rel_pose_3d_with... for 2 frames

parent 49c6d7ff
No related branches found
No related tags found
1 merge request!462Resolve "Subscriber&processor for landmark external detections"
Hide whitespace changes
Inline Side-by-side
test/CMakeLists.txt
+ 0
− 1
View file @ c4f3802e
...@@ -172,7 +172,6 @@ wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp) ...@@ -172,7 +172,6 @@ wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
# FactorRelativePose3dWithExtrinsics class test # 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 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 # FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp) wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
......
test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+ 374
− 322
View file @ c4f3802e
//--------LICENSE_START-------- //--------LICENSE_START--------
// //
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of WOLF // This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify // 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 // 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 // the Free Software Foundation, either version 3 of the License, or
// at your option) any later version. // at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU Lesser General Public License for more details.
// //
// You should have received a copy of the GNU Lesser General Public License // 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/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- //--------LICENSE_END--------
#include <core/factor/factor_relative_pose_3d_with_extrinsics.h> #include "core/utils/utils_gtest.h"
#include <core/utils/utils_gtest.h>
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/common/wolf.h" #include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
#include "core/utils/logging.h" #include "core/capture/capture_odom_3d.h"
#include "core/sensor/sensor_odom_3d.h"
#include "core/state_block/state_block_derived.h" #include "core/math/rotations.h"
#include "core/state_block/state_quaternion.h" #include "core/state_block/state_block_derived.h"
#include "core/ceres_wrapper/solver_ceres.h" #include "core/state_block/state_quaternion.h"
#include "core/capture/capture_pose.h"
#include "core/feature/feature_pose.h"
using namespace Eigen;
using namespace wolf;
using std::cout;
using namespace Eigen; using std::endl;
using namespace wolf;
using std::static_pointer_cast; std::string wolf_root = _WOLF_ROOT_DIR;
int N = 1e2;
// Use the following in case you want to initialize tests with predefines variables or methods. // Vectors
class FactorRelativePose3dWithExtrinsics_class : public testing::Test{ Vector7d delta, x_0, x_1, x_f, x_l, x_s;
public:
Vector3d pos_camera, pos_robot, pos_landmark; // Input odometry data and covariance
Vector3d euler_camera, euler_robot, euler_landmark; Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
Quaterniond quat_camera, quat_robot, quat_landmark;
Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors // Problem and solver
Vector7d pose_camera, pose_robot, pose_landmark; ProblemPtr problem_ptr = Problem::create("PO", 3);
ProblemPtr problem; SolverCeres solver(problem_ptr);
SolverCeresPtr solver;
// Sensor
SensorBasePtr S; 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");
FrameBasePtr F1;
CapturePosePtr c1; // Two frames
FeaturePosePtr f1; FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
LandmarkBasePtr lmk1; FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
FactorRelativePose3dWithExtrinsicsPtr fac;
// Landmark
Eigen::Matrix6d meas_cov; LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
"LandmarkPose3d",
void SetUp() override 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 // random setup
generateRandomProblemFrame();
/* We have three poses to take into account:
* - pose of the sensor (extrinsincs) ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
* - pose of the landmark
* - pose of the robot (Keyframe) // Perturb frm1, fix the rest
* frm0->fix();
* The measurement provides the pose of the landmark wrt sensor's current pose in the world. frm1->unfix();
* Camera's current pose in World is the composition of the robot pose with the sensor extrinsics. sensor_odom3d->getP()->fix();
* sensor_odom3d->getO()->fix();
* The robot has a sensor looking forward frm1->perturb(1);
* S: pos = (0,0,0), ori = (0, 0, 0)
* // solve for frm1
* There is a point at the origin std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
* P: pos = (0,0,0) //WOLF_INFO(report);
*
* Therefore, P projects exactly at the origin of the sensor, ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
* f: p = (0,0)
* // remove feature (and factor) for the next loop
* We form a Wolf tree with 1 frames F1, 1 capture C1, fea->remove();
* 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1: }
* }
* Frame F1, Capture C1, feature f1, landmark l1, constraint c1
* TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
* The frame pose F1, and the sensor pose S {
* in the robot frame are variables subject to optimization for (int i = 0; i < N; i++)
* {
* We perform a number of tests based on this configuration.*/ // random setup
generateRandomProblemFrame();
// sensor is at origin of robot and looking forward ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
// robot is at (0,0,0)
// landmark is right in front of sensor. Its orientation wrt sensor is identity. // Perturb frm0, fix the rest
pos_camera << 0,0,0; frm1->fix();
pos_robot << 0,0,0; //robot is at origin frm0->unfix();
pos_landmark << 0,1,0; sensor_odom3d->getP()->fix();
euler_camera << 0,0,0; sensor_odom3d->getO()->fix();
//euler_camera << -M_PI_2, 0, -M_PI_2; frm0->perturb(1);
euler_robot << 0,0,0;
euler_landmark << 0,0,0; // solve for frm0
quat_camera = e2q(euler_camera); std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
quat_robot = e2q(euler_robot); //WOLF_INFO(report);
quat_landmark = e2q(euler_landmark);
vquat_camera = quat_camera.coeffs(); ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-3);
vquat_robot = quat_robot.coeffs();
vquat_landmark = quat_landmark.coeffs(); // remove feature (and factor) for the next loop
pose_camera << pos_camera, vquat_camera; fea->remove();
pose_robot << pos_robot, vquat_robot; }
pose_landmark << pos_landmark, vquat_landmark; }
// Build problem // JV: The position extrinsics in case of pair of frames apears to be not very observable
problem = Problem::create("PO", 3); TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
solver = std::make_shared<SolverCeres>(problem); {
for (int i = 0; i < N; i++)
// Create sensor to be able to initialize (a camera for instance) {
S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose", // random setup
std::make_shared<StatePoint3d>(pos_camera, true), generateRandomProblemFrame();
std::make_shared<StateQuaternion>(vquat_camera, true),
std::make_shared<StateParams4>(Vector4d::Zero(), false), // fixed ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
Vector1d::Zero());
// Perturb sensor P, fix the rest
// ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>(); frm1->fix();
// S = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera); frm0->fix();
// camera = std::static_pointer_cast<SensorCamera>(S); sensor_odom3d->getP()->unfix();
sensor_odom3d->getO()->fix();
//WOLF_INFO("sensor P before perturbing: ", sensor_odom3d->getP()->getState().transpose());
// F1 is be origin KF sensor_odom3d->getP()->perturb(1);
VectorComposite x0(pose_robot, "PO", {3,4}); //WOLF_INFO("sensor P after perturbing: ", sensor_odom3d->getP()->getState().transpose());
VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
F1 = problem->setPriorFactor(x0, s0, 0.0); // solve for frm0
std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
//WOLF_INFO("sensor P after solving: ", sensor_odom3d->getP()->getState().transpose());
// 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) //WOLF_INFO(report);
meas_cov = Eigen::Matrix6d::Identity();
meas_cov.topLeftCorner(3,3) *= 1e-2; //ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3);
meas_cov.bottomRightCorner(3,3) *= 1e-3;
// remove feature (and factor) for the next loop
//emplace feature and landmark fea->remove();
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), TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark))); {
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();
} }
}; }
// FRAME TO LANDMARK =========================================================================
TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor) TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
{ {
auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1, for (int i = 0; i < N; i++)
lmk1, {
nullptr, // random setup
false); generateRandomProblemLandmark();
ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics"); ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
}
// Perturb landmark, fix the rest
///////////////////////////////////////////// frm1->fix();
// Tree not ok with this gtest implementation lmk->unfix();
///////////////////////////////////////////// sensor_odom3d->getP()->fix();
TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree) sensor_odom3d->getO()->fix();
{ lmk->perturb(1);
ASSERT_TRUE(problem->check(1));
// solve for landmark
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
f1, //WOLF_INFO(report);
lmk1,
nullptr, ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-3);
false);
ASSERT_TRUE(problem->check(1)); // remove feature (and factor) for the next loop
} fea->remove();
}
TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated) }
{
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
f1, {
lmk1, for (int i = 0; i < N; i++)
nullptr, {
false); // random setup
generateRandomProblemLandmark();
// unfix F1, perturbate state
F1->unfix(); ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
F1->getP()->perturb();
// Perturb frm0, fix the rest
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport frm1->unfix();
ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); lmk->fix();
} sensor_odom3d->getP()->fix();
sensor_odom3d->getO()->fix();
TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated) frm1->perturb(1);
{
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, // solve for frm0
f1, std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
lmk1, //WOLF_INFO(report);
nullptr,
false); ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-3);
// unfix F1, perturbate state // remove feature (and factor) for the next loop
F1->unfix(); fea->remove();
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(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
} {
for (int i = 0; i < N; i++)
TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization) {
{ // random setup
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, generateRandomProblemLandmark();
f1,
lmk1, ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
nullptr,
false); // Perturb sensor P, fix the rest
frm1->fix();
ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); lmk->fix();
ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6); sensor_odom3d->getP()->unfix();
ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); sensor_odom3d->getO()->fix();
sensor_odom3d->getP()->perturb(1);
}
// solve for frm0
TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated) std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
{ //WOLF_INFO(report);
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
f1, ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3);
lmk1,
nullptr, // remove feature (and factor) for the next loop
false); fea->remove();
}
// unfix lmk1, perturbate state }
lmk1->unfix();
lmk1->getP()->perturb(); TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
{
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport for (int i = 0; i < N; i++)
ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); {
ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); // random setup
} generateRandomProblemLandmark();
TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated) ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
{
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, // Perturb sensor O, fix the rest
f1, frm1->fix();
lmk1, lmk->fix();
nullptr, sensor_odom3d->getP()->fix();
false); sensor_odom3d->getO()->unfix();
sensor_odom3d->getO()->perturb(.2);
// unfix F1, perturbate state
lmk1->unfix(); //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl;
lmk1->getO()->perturb();
// solve for frm0
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); //WOLF_INFO(report);
ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3);
}
// remove feature (and factor) for the next loop
TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated) fea->remove();
{ }
// 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; int main(int argc, char **argv)
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)
{
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp deleted 100644 → 0
+ 0
− 377
View file @ 49c6d7ff
//--------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();
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment