Skip to content
Snippets Groups Projects
Commit 4cc2ccb6 authored by Víctor Sainz Ubide's avatar Víctor Sainz Ubide
Browse files

added test WIP

parent 214fa9cf
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
// wolf
#include "laser/processor/processor_odom_icp_3d.h"
#include "core/processor/factory_processor.h"
// wolf yaml
#include <core/utils/params_server.h>
#include <core/yaml/parser_yaml.h>
#include "core/yaml/yaml_conversion.h"
// testing
#include <core/utils/utils_gtest.h>
#include <core/utils/logging.h>
using namespace wolf;
using namespace laser;
WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
{
public:
ProblemPtr P;
SensorLaser3dPtr S;
ProcessorOdomIcp3dPtr p;
FrameBasePtr F0, F1, F;
CaptureLaser3dPtr C0, C1, C;
FeatureBasePtr f1;
FactorBasePtr c1;
VectorXd data;
MatrixXd data_cov;
VectorXd delta;
MatrixXd delta_cov;
Matrix<double, 13, 1> calib;
MatrixXd J_delta_calib;
protected:
void SetUp() override
{
std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
ParserYaml parser = ParserYaml("processor_odom_icp_3d.yaml", wolf_root + "/test/yaml/");
ParamsServer server = ParamsServer(parser.getParams());
P = Problem::autoSetup(server);
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
data = VectorXd::Zero(12); // [ a, w, f, t ]
data_cov = MatrixXd::Identity(12, 12);
C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
C->process();
C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
F0 = C0->getFrame();
ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
C->process();
C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
F1 = C1->getFrame();
F1->link(P); // by the face
VectorXd delta_preint(VectorXd::Zero(19));
delta_preint.head<3>() = -0.5 * gravity();
delta_preint.segment<3>(3) = -gravity();
delta_preint.segment<3>(6) = -0.5 * gravity();
delta_preint.segment<3>(9) = -gravity();
delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
f1 = FeatureBase::emplace<FeatureMotion>(
C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
F1->getStateBlock('P')->setState(Vector3d(0, 0, 0));
F1->getStateBlock('V')->setState(Vector3d(0, 0, 0));
F1->getStateBlock('O')->setState(Vector4d(0, 0, 0, 1));
F1->getStateBlock('L')->setState(Vector3d(0, 0, 0));
}
};
class Test_FactorForceTorqueInertialDynamics : public testing::Test
{
public:
ProblemPtr P;
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialPreintDynamicsPtr p;
FrameBasePtr F0, F1;
CaptureMotionPtr C0, C1;
FeatureMotionPtr f1;
FactorForceTorqueInertialDynamicsPtr c1;
VectorXd data;
MatrixXd data_cov;
VectorXd delta;
MatrixXd delta_cov;
Matrix<double, 13, 1> calib;
MatrixXd J_delta_calib;
protected:
void SetUp() override
{
data = VectorXd::Zero(12); // [ a, w, f, t ]
data_cov = MatrixXd::Identity(12, 12);
// crear un problem
P = Problem::create("POVL", 3);
// crear un sensor
auto params_sensor = std::make_shared<ParamsSensorForceTorqueInertial>();
Vector7d extrinsics;
extrinsics << 0, 0, 0, 0, 0, 0, 1;
S = SensorBase::emplace<SensorForceTorqueInertial>(P->getHardware(), extrinsics, params_sensor);
S->setStateBlockDynamic('I', true);
// crear processador
auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialPreintDynamics>();
p = ProcessorBase::emplace<ProcessorForceTorqueInertialPreintDynamics>(S, params_processor);
// crear dos frame
VectorXd state(13);
state << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
VectorComposite state_c(state, "POVL", {3, 4, 3, 3});
F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 0.0, "POVL", state_c);
F1 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 1.0, "POVL", state_c);
// crear dues capture
VectorXd data(VectorXd::Zero(12));
MatrixXd data_cov(MatrixXd::Identity(12, 12));
Vector6d bias(Vector6d::Zero());
auto sb_bias = std::make_shared<StateParams6>(bias);
C0 = CaptureBase::emplace<CaptureMotion>(
F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr, nullptr, sb_bias);
C1 = CaptureBase::emplace<CaptureMotion>(
F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr, sb_bias);
// crear un feature
VectorXd delta_preint(VectorXd::Zero(19));
delta_preint.head<3>() = -0.5 * gravity();
delta_preint.segment<3>(3) = -gravity();
delta_preint.segment<3>(6) = -0.5 * gravity();
delta_preint.segment<3>(9) = -gravity();
delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
f1 = FeatureBase::emplace<FeatureMotion>(
C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
// crear un factor
c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
}
};
// TEST(FactorForceTorqueInertialDynamics_yaml, force_registraion)
// {
// FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
// }
// Test to see if the constructor works (not yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics, constructor)
{
ASSERT_EQ(c1->getCapture(), C1);
ASSERT_EQ(c1->getCalibPreint().size(), 13);
}
// Test to see if the constructor works (yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
{
ASSERT_EQ(c1->getCapture(), C1);
ASSERT_EQ(c1->getCalibPreint().size(), 13);
}
// Test en el que no hi ha moviment (not yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
{
VectorXd res_exp = VectorXd::Zero(18);
Matrix<double, 18, 1> res;
VectorXd bias = VectorXd::Zero(6);
P->print(4, 1, 1, 1);
c1->residual(F0->getStateBlock('P')->getState(), // p0
Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
F0->getStateBlock('V')->getState(), // v0
F0->getStateBlock('L')->getState(), // L0
bias, // I
F1->getStateBlock('P')->getState(), // p1
Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
F1->getStateBlock('V')->getState(), // v1
F1->getStateBlock('L')->getState(), // L1
S->getCom(), // C
S->getInertia(), // i
S->getMass(), // m
res);
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
// Test en el que no hi ha moviment (yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
{
VectorXd res_exp = VectorXd::Zero(18);
Matrix<double, 18, 1> res;
VectorXd bias = VectorXd::Zero(6);
c1->residual(F0->getStateBlock('P')->getState(), // p0
Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
F0->getStateBlock('V')->getState(), // v0
F0->getStateBlock('L')->getState(), // L0
bias, // I
F1->getStateBlock('P')->getState(), // p1
Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
F1->getStateBlock('V')->getState(), // v1
F1->getStateBlock('L')->getState(), // L1
S->getCom(), // C
S->getInertia(), // i
S->getMass(), // m
res);
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
// Test en el que s'avança 1m en direcció x (not yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
{
VectorXd res_exp = VectorXd::Zero(18);
Matrix<double, 18, 1> res;
VectorXd bias = VectorXd::Zero(6);
// provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
// Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
VectorXd delta_preint(VectorXd::Zero(19));
delta_preint.head<3>() = -0.5 * gravity();
delta_preint(0) = 1;
delta_preint.segment<3>(3) = -gravity();
delta_preint.segment<3>(6) = -0.5 * gravity();
delta_preint(6) = 1;
delta_preint.segment<3>(9) = -gravity();
delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
FactorForceTorqueInertialDynamicsPtr c2 =
FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
c2->residual(F0->getStateBlock('P')->getState(), // p0
Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
F0->getStateBlock('V')->getState(), // v0
F0->getStateBlock('L')->getState(), // L0
bias, // I
F1->getStateBlock('P')->getState(), // p1
Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
F1->getStateBlock('V')->getState(), // v1
F1->getStateBlock('L')->getState(), // L1
S->getCom(), // C
S->getInertia(), // i
S->getMass(), // m
res);
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
// Test en el que s'avança 1m en direcció x (fitxers yaml)
TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
{
VectorXd res_exp = VectorXd::Zero(18);
Matrix<double, 18, 1> res;
VectorXd bias = VectorXd::Zero(6);
// provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
// Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
VectorXd delta_preint(VectorXd::Zero(19));
delta_preint.head<3>() = -0.5 * gravity();
delta_preint(0) = 1;
delta_preint.segment<3>(3) = -gravity();
delta_preint.segment<3>(6) = -0.5 * gravity();
delta_preint(6) = 1;
delta_preint.segment<3>(9) = -gravity();
delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
FactorForceTorqueInertialDynamicsPtr c2 =
FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
c2->residual(F0->getStateBlock('P')->getState(), // p0
Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
F0->getStateBlock('V')->getState(), // v0
F0->getStateBlock('L')->getState(), // L0
bias, // I
F1->getStateBlock('P')->getState(), // p1
Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
F1->getStateBlock('V')->getState(), // v1
F1->getStateBlock('L')->getState(), // L1
S->getCom(), // C
S->getInertia(), // i
S->getMass(), // m
res);
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
//::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
return RUN_ALL_TESTS();
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment