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"
Expand all 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)
# 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)
......
test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+ 374
− 322
View file @ c4f3802e
This diff is collapsed.
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