// tech report:
//
static void func_jacob_expe_D(
const CArrayDouble<6> &eps,
const CPose3D &D, CArrayDouble<12> &Y)
{
CPose3D incr;
CPose3D::exp(eps,incr);
const CPose3D expe_D = incr + D;
expe_D.getAs12Vector(Y);
}
static void func_jacob_Aexpe_D(
const CArrayDouble<6> &eps,
const TParams_func_jacob_Aexpe_D ¶ms, CArrayDouble<12> &Y)
{
CPose3D incr;
CPose3D::exp(eps,incr);
const CPose3D res = params.A + incr + params.D;
res.getAs12Vector(Y);
}
static void func_jacob_expe_e(
const CArrayDouble<6> &x,
const double &dummy, CArrayDouble<12> &Y)
{
MRPT_UNUSED_PARAM(dummy);
const CPose3D p = CPose3D::exp(x);
//const CMatrixDouble44 R = p.getHomogeneousMatrixVal();
p.getAs12Vector(Y);
}
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);
}
}
