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