path_from_rtk_gps_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include <mrpt/topography.h>
#include <mrpt/poses/CPose3DInterpolator.h>
#include <mrpt/system/filesystem.h>
#include <gtest/gtest.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::topography;
using namespace std;

// Defined in tests/test_main.cpp
namespace mrpt
{
namespace utils
{
extern std::string MRPT_GLOBAL_UNITTEST_SRC_DIR;
}
}

TEST(TopographyReconstructPathFrom3RTK, sampleDataset)
{
        mrpt::poses::CPose3DInterpolator robot_path;

        mrpt::obs::CRawlog rawlog;

        const string dataset_fil =
                MRPT_GLOBAL_UNITTEST_SRC_DIR +
                string("/share/mrpt/datasets/test_rtk_path.rawlog");
        if (!mrpt::system::fileExists(dataset_fil))
        {
                cerr << "WARNING: Skipping test due to missing file: " << dataset_fil
                         << "\n";
                return;
        }
        if (!rawlog.loadFromRawLogFile(dataset_fil))
        {
                cerr << "WARNING: Skipping test due to error loading file: "
                         << dataset_fil << "\n";
        }
        else
        {
                mrpt::topography::TPathFromRTKInfo rtk_path_info;

                // -------------------------------------------
                // Run path reconstruction:
                // -------------------------------------------
                mrpt::topography::path_from_rtk_gps(
                        robot_path, rawlog,
                        0,  // first entry
                        rawlog.size() - 1,  // last entry
                        false,  // Isn't a GUI
                        false,  // disableGPSInterp
                        1,  // path_smooth_filter_size
                        &rtk_path_info);

                EXPECT_EQ(robot_path.size(), 75u);

                // Expected values:
                // 1226225355.000000 279.705647 216.651473 8.517821 0.194222 -0.083873
                // -0.045293
                // 1226225380.000000 377.095830 233.343569 9.724171 0.177037 -0.073565
                // -0.019024
                const mrpt::system::TTimeStamp t1 =
                        mrpt::system::time_tToTimestamp(1226225355.000000);
                const mrpt::system::TTimeStamp t2 =
                        mrpt::system::time_tToTimestamp(1226225380.000000);
                const CPose3D pose_GT_1(
                        279.696, 216.623, 9.21315, 0.195764, -0.0319733, -0.0420478);
                const CPose3D pose_GT_2(
                        377.087, 233.311, 10.474, 0.178932, -0.0212096, -0.0154982);

                CPose3D pose1, pose2;
                bool valid;
                robot_path.interpolate(t1, pose1, valid);
                EXPECT_TRUE(valid);

                robot_path.interpolate(t2, pose2, valid);
                EXPECT_TRUE(valid);

                CVectorDouble p1vec(12), p2vec(12);
                pose1.getAs12Vector(p1vec);
                pose2.getAs12Vector(p2vec);

                CVectorDouble p1vec_gt(12), p2vec_gt(12);
                pose_GT_1.getAs12Vector(p1vec_gt);
                pose_GT_2.getAs12Vector(p2vec_gt);

                EXPECT_NEAR((p1vec - p1vec_gt).array().abs().sum(), 0, 1e-3);
                EXPECT_NEAR((p2vec - p2vec_gt).array().abs().sum(), 0, 1e-3);
        }
}