diff --git a/CMakeLists.txt b/CMakeLists.txt index 1886fd5119aa1aaba3259fb5fcbfc1832e34bdbc..a90ee5e67b96e6cca9df6bc5db611725df3abd46 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -233,8 +233,11 @@ if(PCL_FOUND) SET(SRCS_CAPTURE ${SRCS_CAPTURE} src/capture/capture_laser_3d.cpp ) + SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} + src/processor/processor_odom_icp_3d.cpp + ) SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} - src/processor/processor_odom_icp_3d.cpp + src/sensor/sensor_laser_3d.cpp ) endif(PCL_FOUND) diff --git a/include/laser/sensor/sensor_laser_3d.h b/include/laser/sensor/sensor_laser_3d.h index 9b9ca106bfbe6f220ea1e429ee140807d32528ec..bf077535c64c7c1f2638c8947816dc2c31b78306 100644 --- a/include/laser/sensor/sensor_laser_3d.h +++ b/include/laser/sensor/sensor_laser_3d.h @@ -33,7 +33,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d); struct ParamsSensorLaser3d : public ParamsSensorBase { ParamsSensorLaser3d() = default; - ParamsSensorLaser3d(std::string _unique_name, const wolf::ParamsServer& _server) + ParamsSensorLaser3d(std::string _unique_name, const ParamsServer& _server) : ParamsSensorBase(_unique_name, _server) { } @@ -46,7 +46,7 @@ class SensorLaser3d : public SensorBase { public: SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr); - SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params); + SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params); ~SensorLaser3d(); WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 7); ParamsSensorLaser3dPtr params_laser3d_; diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp index c9104c9026b20e1f28ccb4f1d408851248e2c255..20dde002124334b5c64cf8241cd3fbdbba92a3e0 100644 --- a/src/processor/processor_odom_icp_3d.cpp +++ b/src/processor/processor_odom_icp_3d.cpp @@ -86,11 +86,14 @@ void ProcessorOdomIcp3d::preProcess() */ unsigned int ProcessorOdomIcp3d::processKnown() { - pairAlign(origin_laser_->getPointCloud(), - incoming_laser_->getPointCloud(), - T_origin_last_, - T_origin_incoming_, - registration_solver_); + if (origin_ptr_ && (incoming_ptr_ != origin_ptr_)) + { + pairAlign(origin_laser_->getPointCloud(), + incoming_laser_->getPointCloud(), + T_origin_last_, + T_origin_incoming_, + registration_solver_); + } return 0; }; @@ -186,4 +189,10 @@ void ProcessorOdomIcp3d::establishFactors() feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION); }; -} // namespace wolf \ No newline at end of file +} // namespace wolf +#include "core/processor/factory_processor.h" +namespace wolf +{ +WOLF_REGISTER_PROCESSOR(ProcessorOdomIcp3d); +WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdomIcp3d); +} // namespace wolf diff --git a/src/sensor/sensor_laser_3d.cpp b/src/sensor/sensor_laser_3d.cpp index 1cb46b8b355aa6df5d6b37e9228d10e4228f1fc2..40d0bda87dc37a8efa5a7378a085f3bd788ce5fd 100644 --- a/src/sensor/sensor_laser_3d.cpp +++ b/src/sensor/sensor_laser_3d.cpp @@ -21,6 +21,9 @@ //--------LICENSE_END-------- #include "laser/sensor/sensor_laser_3d.h" +#include <core/state_block/state_block_derived.h> +#include <core/state_block/state_quaternion.h> + namespace wolf { @@ -29,17 +32,25 @@ SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) { } -SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params); -SensorBase("SensorLaser3d", - std::make_shared<StatePoint3d>(_extrinsics.head(3), true), - std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), - nullptr, - 0), - params_laser3d_(_params) +SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params) + : SensorBase("SensorLaser3d", + std::make_shared<StatePoint3d>(_extrinsics.head(3), true), + std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), + nullptr, + 0), + params_laser3d_(_params) { // } SensorLaser3d::~SensorLaser3d() {} -} // namespace wolf \ No newline at end of file +} // namespace wolf + +// Register in the FactorySensor and the ParameterFactory +#include "core/sensor/factory_sensor.h" +namespace wolf { +WOLF_REGISTER_SENSOR(SensorLaser3d) +WOLF_REGISTER_SENSOR_AUTO(SensorLaser3d) +} // namespace wolf + diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp index 0cb18a1fb3a270ef879cac22b38f1b1455596018..2a3378532ad8041c9664483496f4f1dee22a8956 100644 --- a/test/gtest_processor_odom_icp_3d.cpp +++ b/test/gtest_processor_odom_icp_3d.cpp @@ -19,10 +19,16 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -// wolf + +// wolf laser +#include "laser/sensor/sensor_laser_3d.h" #include "laser/processor/processor_odom_icp_3d.h" -#include "core/processor/factory_processor.h" +#include "laser/internal/config.h" + +// wolf core #include "core/common/wolf.h" +#include <core/sensor/factory_sensor.h> +#include "core/processor/factory_processor.h" // wolf yaml #include <core/utils/params_server.h> @@ -33,10 +39,23 @@ #include <core/utils/utils_gtest.h> #include <core/utils/logging.h> + +// pcl includes +#include <pcl/point_cloud.h> +#include <pcl/io/pcd_io.h> +#include <pcl/filters/voxel_grid.h> + using namespace wolf; -using namespace laser; -WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d); +std::string laser_root_dir = _WOLF_LASER_ROOT_DIR; + +void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud) +{ + pcl::io::loadPCDFile(fname, cloud); + // remove NAN points from the cloud + pcl::Indices indices; + pcl::removeNaNFromPointCloud(cloud, cloud, indices); +}; class Test_ProcessorOdomIcp3D_yaml : public testing::Test { @@ -46,22 +65,17 @@ class Test_ProcessorOdomIcp3D_yaml : public testing::Test ProcessorOdomIcp3dPtr p; FrameBasePtr F0, F1, F; CaptureLaser3dPtr C0, C1, C2; - FeatureBasePtr f1, f2, f3; - FactorBasePtr c1, c2; - + 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 laser_root_dir = _WOLF_LASER_ROOT_DIR; - ParserYaml parser = ParserYaml("processor_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/"); + WOLF_INFO(laser_root_dir); + + ParserYaml parser = ParserYaml("problem_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/"); ParamsServer server = ParamsServer(parser.getParams()); P = Problem::autoSetup(server); @@ -112,239 +126,197 @@ class Test_ProcessorOdomIcp3D_yaml : public testing::Test */ } }; + -class Test_ProcessorOdomIcp3D : public testing::Test +TEST(Init, register_in_factories) { - public: - ProblemPtr P; - SensorForceTorqueInertialPtr S; - ProcessorForceTorqueInertialPreintDynamicsPtr p; - FrameBasePtr F0, F1; - CaptureMotionPtr C0, C1; - FeatureMotionPtr f1; - FactorForceTorqueInertialDynamicsPtr c1; +FactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create); +AutoConfFactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create); +FactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create); +AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create); - 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); +TEST_F(Test_ProcessorOdomIcp3D_yaml, constructor) +{ + // Load data + pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref1 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); + loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref1); - // 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); + C0 = std::make_shared<CaptureLaser3d>(0.0, S, pcl_ref1); - // crear un factor - c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false); - } -}; + C0->process(); -// TEST(FactorForceTorqueInertialDynamics_yaml, force_registraion) -// { -// FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create); -// } + P->print(4,1,1,1); -// 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); -} + // Load data + pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref2 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); + loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref2); -// 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); -} + C1 = std::make_shared<CaptureLaser3d>(1.0, S, pcl_ref2); -// 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); -} + C1->process(); -// 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); -} + P->print(4,1,1,1); -// 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 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_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); -} +// // 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) { diff --git a/test/yaml/processor_odom_icp_3d.yaml b/test/yaml/problem_odom_icp_3d.yaml similarity index 66% rename from test/yaml/processor_odom_icp_3d.yaml rename to test/yaml/problem_odom_icp_3d.yaml index 82386dd0f0291e5a604a24bb9d5a0902a880b16c..835d2077f19bdb6af2b96f9e85ff6f3903eac19a 100644 --- a/test/yaml/processor_odom_icp_3d.yaml +++ b/test/yaml/problem_odom_icp_3d.yaml @@ -7,6 +7,9 @@ config: $state: P: [0,0,0] O: [0,0,0,1] + $sigma: + P: [.1,.1,.1] + O: [.1,.1,.1] time_tolerance: 0.05 tree_manager: type: "None" @@ -31,4 +34,15 @@ config: icp_max_iterations: 20 icp_transformation_rotation_epsilon: 0.99 icp_transformation_translation_epsilon: 1e-6 - icp_max_correspondence_distance: 0.5 \ No newline at end of file + icp_max_correspondence_distance: 0.5 + keyframe_vote: + voting_active: false + min_features_for_keyframe : 0 + apply_loss_function: false + max_new_features: 0 + state_getter: true + state_priority: 1 + pcl_downsample: true + vote_elapsed_time: 0.99 + +