Skip to content
Snippets Groups Projects
Commit bd4f9511 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Make test with a for loop to make more KFs

parent 7255c15e
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
...@@ -39,7 +39,6 @@ ...@@ -39,7 +39,6 @@
#include <core/utils/utils_gtest.h> #include <core/utils/utils_gtest.h>
#include <core/utils/logging.h> #include <core/utils/logging.h>
// pcl includes // pcl includes
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
...@@ -60,121 +59,68 @@ void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud) ...@@ -60,121 +59,68 @@ void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud)
class Test_ProcessorOdomIcp3D_yaml : public testing::Test class Test_ProcessorOdomIcp3D_yaml : public testing::Test
{ {
public: public:
ProblemPtr P; ProblemPtr P;
SensorLaser3dPtr S; SensorLaser3dPtr S;
ProcessorOdomIcp3dPtr p; ProcessorOdomIcp3dPtr p;
FrameBasePtr F0, F1, F; FrameBasePtr F0, F1, F;
CaptureLaser3dPtr C0, C1, C2, C3; CaptureLaser3dPtr C0, C1, C2, C3;
VectorXd data; VectorXd data;
MatrixXd data_cov; MatrixXd data_cov;
protected: protected:
void SetUp() override void SetUp() override
{ {
WOLF_INFO(laser_root_dir); WOLF_INFO(laser_root_dir);
ParserYaml parser = ParserYaml("problem_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/"); ParserYaml parser = ParserYaml("problem_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/");
ParamsServer server = ParamsServer(parser.getParams()); ParamsServer server = ParamsServer(parser.getParams());
P = Problem::autoSetup(server); P = Problem::autoSetup(server);
S = std::static_pointer_cast<SensorLaser3d>(P->getHardware()->getSensorList().front()); S = std::static_pointer_cast<SensorLaser3d>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorOdomIcp3d>(S->getProcessorList().front()); p = std::static_pointer_cast<ProcessorOdomIcp3d>(S->getProcessorList().front());
P->print(4, 1, 1, 1);
P->print(4,1,1,1);
/*
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));
*/
} }
}; };
TEST(Init, register_in_factories) TEST(Init, register_in_factories)
{ {
FactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create); FactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
AutoConfFactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create); AutoConfFactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
FactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create); FactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create); AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
} }
TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform) TEST_F(Test_ProcessorOdomIcp3D_yaml, process_make_keyframes_always_same_pcl)
{ {
// Load data std::vector<VectorComposite> X(10);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref0 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); unsigned int i = 0;
loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref0); for (TimeStamp t = 0; t < 6; t += 0.5) // will make one KF every 2 captures
C0 = std::make_shared<CaptureLaser3d>(0.0, S, pcl_ref0); {
C0->process(); // Load pointcloud from file
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref0 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
VectorComposite x0 = P->getState(); loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref0);
// Load data // make Capture and process
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref1 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); C0 = std::make_shared<CaptureLaser3d>(t, S, pcl_ref0);
loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref1); C0->process();
C1 = std::make_shared<CaptureLaser3d>(1.0, S, pcl_ref1);
C1->process(); // store state at integer timestamps (where the KFs are)
if (std::fabs(t.get() - (double)i) < 0.1)
VectorComposite x1 = P->getState(); {
X[i] = p->getLast()->getFrame()->getState("PO");
// Load data i++;
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref2 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); }
loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref2); }
C2 = std::make_shared<CaptureLaser3d>(2.0, S, pcl_ref2);
C2->process(); P->print(4, 1, 1, 1);
VectorComposite x2 = P->getState(); ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(1).vector("PO"), 1e-8); // pointclouds are the same
ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(2).vector("PO"), 1e-8); // pointclouds are the same
// Load data ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(3).vector("PO"), 1e-8); // pointclouds are the same
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref3 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(4).vector("PO"), 1e-8); // pointclouds are the same
loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref3); ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(5).vector("PO"), 1e-8); // pointclouds are the same
C3 = std::make_shared<CaptureLaser3d>(3.0, S, pcl_ref3);
C3->process();
VectorComposite x3 = P->getState();
ASSERT_MATRIX_APPROX(x1.vector("PO"), x2.vector("PO"), 1e-8); // pointclouds 1 and 2 are the same
ASSERT_MATRIX_APPROX(x2.vector("PO"), x3.vector("PO"), 1e-8); // pointclouds 2 and 3 are the same
P->print(4,1,1,1);
} }
// // Test to see if the constructor works (not yaml files) // // Test to see if the constructor works (not yaml files)
...@@ -332,7 +278,7 @@ TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform) ...@@ -332,7 +278,7 @@ TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform)
// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8); // ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
// } // }
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
//::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual"; //::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
......
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