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

test added & factory registered

parent 368c75a3
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
......@@ -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)
......
......@@ -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_;
......
......@@ -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
......@@ -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
......@@ -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)
{
......
......@@ -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
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