path_from_rtk_gps.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 "topography-precomp.h"  // Precompiled headers
 
#include <mrpt/tfest/se3.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/topography/data_types.h>
#include <mrpt/topography/conversions.h>
#include <mrpt/topography/path_from_rtk_gps.h>
 
#include <memory>
 
#if MRPT_HAS_WXWIDGETS
#include <wx/app.h>
#include <wx/msgdlg.h>
#include <wx/string.h>
#include <wx/progdlg.h>
#include <wx/busyinfo.h>
#include <wx/log.h>
#endif  // MRPT_HAS_WXWIDGETS
 
using namespace std;
using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace mrpt::tfest;
using namespace mrpt::config;
using namespace mrpt::topography;
 
template <class T>
std::set<T> make_set(const T& v0, const T& v1)
{
    std::set<T> s;
    s.insert(v0);
    s.insert(v1);
    return s;
}
 
/*---------------------------------------------------------------
                    path_from_rtk_gps
 ---------------------------------------------------------------*/
void mrpt::topography::path_from_rtk_gps(
    mrpt::poses::CPose3DInterpolator& robot_path,
    const mrpt::obs::CRawlog& rawlog, size_t first, size_t last, bool isGUI,
    bool disableGPSInterp, int PATH_SMOOTH_FILTER, TPathFromRTKInfo* outInfo)
{
    MRPT_START
 
#if MRPT_HAS_WXWIDGETS
    // Use a smart pointer so we are safe against exceptions:
    std::unique_ptr<wxBusyCursor> waitCursorPtr;
    if (isGUI)
        waitCursorPtr = std::unique_ptr<wxBusyCursor>(new wxBusyCursor());
#else
    MRPT_UNUSED_PARAM(isGUI);
#endif
 
    // Go: generate the map:
    ASSERT_(first <= last);
    ASSERT_(last <= rawlog.size() - 1);
 
    set<string> lstGPSLabels;
 
    size_t count = 0;
 
    robot_path.clear();
    robot_path.setMaxTimeInterpolation(std::chrono::seconds(
        3));  // Max. seconds of GPS blackout not to interpolate.
    robot_path.setInterpolationMethod(mrpt::poses::imSSLSLL);
 
    TPathFromRTKInfo outInfoTemp;
    if (outInfo) *outInfo = outInfoTemp;
 
    map<string, map<Clock::time_point, TPoint3D>>
        gps_paths;  // label -> (time -> 3D local coords)
 
    bool abort = false;
    bool ref_valid = false;
 
    // Load configuration block:
    CConfigFileMemory memFil;
    rawlog.getCommentTextAsConfigFile(memFil);
 
    TGeodeticCoords ref(
        memFil.read_double("GPS_ORIGIN", "lat_deg", 0),
        memFil.read_double("GPS_ORIGIN", "lon_deg", 0),
        memFil.read_double("GPS_ORIGIN", "height", 0));
 
    ref_valid = !ref.isClear();
 
    // Do we have info for the consistency test?
    const double std_0 = memFil.read_double("CONSISTENCY_TEST", "std_0", 0);
    bool doConsistencyCheck = std_0 > 0;
 
    // Do we have the "reference uncertainty" matrix W^\star ??
    memFil.read_matrix("UNCERTAINTY", "W_star", outInfoTemp.W_star);
    const bool doUncertaintyCovs = outInfoTemp.W_star.rows() != 0;
    if (doUncertaintyCovs &&
        (outInfoTemp.W_star.rows() != 6 || outInfoTemp.W_star.cols() != 6))
        THROW_EXCEPTION(
            "ERROR: W_star matrix for uncertainty estimation is provided but "
            "it's not a 6x6 matrix.");
 
// ------------------------------------------
// Look for the 2 observations:
// ------------------------------------------
#if MRPT_HAS_WXWIDGETS
    wxProgressDialog* progDia = nullptr;
    if (isGUI)
    {
        progDia = new wxProgressDialog(
            wxT("Building map"), wxT("Getting GPS observations..."),
            (int)(last - first + 1),  // range
            nullptr,  // parent
            wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
                wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);
    }
#endif
 
    // The list with all time ordered gps's in valid RTK mode
    using TListGPSs = std::map<
        mrpt::Clock::time_point, std::map<std::string, CObservationGPS::Ptr>>;
    TListGPSs list_gps_obs;
 
    map<string, size_t> GPS_RTK_reads;  // label-># of RTK readings
    map<string, TPoint3D>
        GPS_local_coords_on_vehicle;  // label -> local pose on the vehicle
 
    for (size_t i = first; !abort && i <= last; i++)
    {
        switch (rawlog.getType(i))
        {
            default:
                break;
 
            case CRawlog::etObservation:
            {
                CObservation::Ptr o = rawlog.getAsObservation(i);
 
                if (o->GetRuntimeClass() == CLASS_ID(CObservationGPS))
                {
                    CObservationGPS::Ptr obs =
                        std::dynamic_pointer_cast<CObservationGPS>(o);
 
                    if (obs->has_GGA_datum &&
                        obs->getMsgByClass<gnss::Message_NMEA_GGA>()
                                .fields.fix_quality == 4)
                    {
                        // Add to the list:
                        list_gps_obs[obs->timestamp][obs->sensorLabel] = obs;
 
                        lstGPSLabels.insert(obs->sensorLabel);
                    }
 
                    // Save to GPS paths:
                    if (obs->has_GGA_datum &&
                        (obs->getMsgByClass<gnss::Message_NMEA_GGA>()
                                 .fields.fix_quality == 4 ||
                         obs->getMsgByClass<gnss::Message_NMEA_GGA>()
                                 .fields.fix_quality == 5))
                    {
                        GPS_RTK_reads[obs->sensorLabel]++;
 
                        // map<string,TPoint3D> GPS_local_coords_on_vehicle;  //
                        // label -> local pose on the vehicle
                        if (GPS_local_coords_on_vehicle.find(
                                obs->sensorLabel) ==
                            GPS_local_coords_on_vehicle.end())
                            GPS_local_coords_on_vehicle[obs->sensorLabel] =
                                TPoint3D(obs->sensorPose.asTPose());
 
                        // map<string, map<Clock::time_point,TPoint3D> >
                        // gps_paths;
                        // //
                        // label -> (time -> 3D local coords)
                        gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D(
                            obs->getMsgByClass<gnss::Message_NMEA_GGA>()
                                .fields.longitude_degrees,
                            obs->getMsgByClass<gnss::Message_NMEA_GGA>()
                                .fields.latitude_degrees,
                            obs->getMsgByClass<gnss::Message_NMEA_GGA>()
                                .fields.altitude_meters);
                    }
                }
            }
            break;
        }  // end switch type
 
        // Show progress:
        if ((count++ % 100) == 0)
        {
#if MRPT_HAS_WXWIDGETS
            if (progDia)
            {
                if (!progDia->Update((int)(i - first))) abort = true;
                wxTheApp->Yield();
            }
#endif
        }
    }  // end for i
 
#if MRPT_HAS_WXWIDGETS
    if (progDia)
    {
        delete progDia;
        progDia = nullptr;
    }
#endif
 
    // -----------------------------------------------------------
    // At this point we already have the sensor positions, thus
    //  we can estimate the covariance matrix D:
    //
    // TODO: Generalize equations for # of GPS > 3
    // -----------------------------------------------------------
    map<set<string>, double> Ad_ij;  // InterGPS distances in 2D
    map<set<string>, double>
        phi_ij;  // Directions on XY of the lines between i-j
    map<string, size_t>
        D_cov_indexes;  // Sensor label-> index in the matrix (first=0, ...)
    map<size_t, string> D_cov_rev_indexes;  // Reverse of D_cov_indexes
 
    CMatrixDouble D_cov;  // square distances cov
    CMatrixDouble D_cov_1;  // square distances cov (inverse)
    CVectorDouble D_mean;  // square distances mean
 
    if (doConsistencyCheck && GPS_local_coords_on_vehicle.size() == 3)
    {
        unsigned int cnt = 0;
        for (map<string, TPoint3D>::iterator i =
                 GPS_local_coords_on_vehicle.begin();
             i != GPS_local_coords_on_vehicle.end(); ++i)
        {
            // Index tables:
            D_cov_indexes[i->first] = cnt;
            D_cov_rev_indexes[cnt] = i->first;
            cnt++;
 
            for (map<string, TPoint3D>::iterator j = i;
                 j != GPS_local_coords_on_vehicle.end(); ++j)
            {
                if (i != j)
                {
                    const TPoint3D& pi = i->second;
                    const TPoint3D& pj = j->second;
                    Ad_ij[make_set(i->first, j->first)] = pi.distanceTo(pj);
                    phi_ij[make_set(i->first, j->first)] =
                        atan2(pj.y - pi.y, pj.x - pi.x);
                }
            }
        }
        ASSERT_(D_cov_indexes.size() == 3 && D_cov_rev_indexes.size() == 3);
 
        D_cov.setSize(D_cov_indexes.size(), D_cov_indexes.size());
        D_mean.resize(D_cov_indexes.size());
 
        // See paper for the formulas!
        // TODO: generalize for N>3
 
        D_cov(0, 0) =
            2 *
            square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
        D_cov(1, 1) =
            2 *
            square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
        D_cov(2, 2) =
            2 *
            square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
 
        D_cov(1, 0) =
            Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
            Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
            cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
                phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
        D_cov(0, 1) = D_cov(1, 0);
 
        D_cov(2, 0) =
            -Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
            Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
            cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
                phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
        D_cov(0, 2) = D_cov(2, 0);
 
        D_cov(2, 1) =
            Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
            Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
            cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] -
                phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
        D_cov(1, 2) = D_cov(2, 1);
 
        D_cov *= 4 * square(std_0);
 
        D_cov_1 = D_cov.inv();
 
        // cout << D_cov.inMatlabFormat() << endl;
 
        D_mean[0] =
            square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
        D_mean[1] =
            square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
        D_mean[2] =
            square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
    }
    else
        doConsistencyCheck = false;
 
    // ------------------------------------------
    // Look for the 2 observations:
    // ------------------------------------------
    int N_GPSs = list_gps_obs.size();
 
    if (N_GPSs)
    {
        // loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs
        //  that skip some readings at some times .0 .2 .4 .6 .8
        if (list_gps_obs.size() > 4)
        {
            TListGPSs::iterator F = list_gps_obs.begin();
            ++F;
            ++F;
            TListGPSs::iterator E = list_gps_obs.end();
            --E;
            --E;
 
            for (TListGPSs::iterator it = F; it != E; ++it)
            {
                // Now check if we have 3 gps with the same time stamp:
                // const size_t N = i->second.size();
                std::map<std::string, CObservationGPS::Ptr>& GPS = it->second;
 
                // Check if any in "lstGPSLabels" is missing here:
                for (set<string>::iterator l = lstGPSLabels.begin();
                     l != lstGPSLabels.end(); ++l)
                {
                    // For each GPS in the current timestamp:
                    bool fnd = (GPS.find(*l) != GPS.end());
 
                    if (fnd) continue;  // this one is present.
 
                    // Ok, we have "*l" missing in the set "*i".
                    // Try to interpolate from neighbors:
                    TListGPSs::iterator i_b1 = it;
                    --i_b1;
                    TListGPSs::iterator i_a1 = it;
                    ++i_a1;
 
                    CObservationGPS::Ptr GPS_b1, GPS_a1;
 
                    if (i_b1->second.find(*l) != i_b1->second.end())
                        GPS_b1 = i_b1->second.find(*l)->second;
 
                    if (i_a1->second.find(*l) != i_a1->second.end())
                        GPS_a1 = i_a1->second.find(*l)->second;
 
                    if (!disableGPSInterp && GPS_a1 && GPS_b1)
                    {
                        if (mrpt::system::timeDifference(
                                GPS_b1->timestamp, GPS_a1->timestamp) < 0.5)
                        {
                            CObservationGPS::Ptr new_gps = CObservationGPS::Ptr(
                                new CObservationGPS(*GPS_a1));
                            new_gps->sensorLabel = *l;
 
                            // cout <<
                            // mrpt::system::timeLocalToString(GPS_b1->timestamp)
                            // << " " <<
                            // mrpt::system::timeLocalToString(GPS_a1->timestamp)
                            // << " " << *l;
                            // cout << endl;
 
                            new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
                                .fields.longitude_degrees =
                                0.5 *
                                (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
                                     .fields.longitude_degrees +
                                 GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
                                     .fields.longitude_degrees);
                            new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
                                .fields.latitude_degrees =
                                0.5 *
                                (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
                                     .fields.latitude_degrees +
                                 GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
                                     .fields.latitude_degrees);
                            new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
                                .fields.altitude_meters =
                                0.5 *
                                (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
                                     .fields.altitude_meters +
                                 GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
                                     .fields.altitude_meters);
 
                            new_gps->timestamp =
                                mrpt::Clock::time_point(mrpt::Clock::duration(
                                    (GPS_a1->timestamp.time_since_epoch()
                                         .count() +
                                     GPS_b1->timestamp.time_since_epoch()
                                         .count()) /
                                    2));
 
                            it->second[new_gps->sensorLabel] = new_gps;
                        }
                    }
                }
            }  // end loop interpolate 1-out-of-5
        }
 
#if MRPT_HAS_WXWIDGETS
        wxProgressDialog* progDia3 = nullptr;
        if (isGUI)
        {
            progDia3 = new wxProgressDialog(
                wxT("Building map"), wxT("Estimating 6D path..."),
                N_GPSs,  // range
                nullptr,  // parent
                wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
                    wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME |
                    wxPD_REMAINING_TIME);
        }
#endif
 
        int idx_in_GPSs = 0;
 
        for (TListGPSs::iterator i = list_gps_obs.begin();
             i != list_gps_obs.end(); ++i, idx_in_GPSs++)
        {
            // Now check if we have 3 gps with the same time stamp:
            if (i->second.size() >= 3)
            {
                const size_t N = i->second.size();
                std::map<std::string, CObservationGPS::Ptr>& GPS = i->second;
                CVectorDouble X(N), Y(N), Z(N);  // Global XYZ coordinates
                std::map<string, size_t>
                    XYZidxs;  // Sensor label -> indices in X Y Z
 
                if (!ref_valid)  // get the reference lat/lon, if it's not set
                // from rawlog configuration block.
                {
                    ref_valid = true;
                    ref = GPS.begin()
                              ->second->getMsgByClass<gnss::Message_NMEA_GGA>()
                              .getAsStruct<TGeodeticCoords>();
                }
 
                // Compute the XYZ coordinates of all sensors:
                TMatchingPairList corrs;
                unsigned int k;
                std::map<std::string, CObservationGPS::Ptr>::iterator g_it;
 
                for (k = 0, g_it = GPS.begin(); g_it != GPS.end(); ++g_it, ++k)
                {
                    TPoint3D P;
                    mrpt::topography::geodeticToENU_WGS84(
                        g_it->second->getMsgByClass<gnss::Message_NMEA_GGA>()
                            .getAsStruct<TGeodeticCoords>(),
                        P, ref);
 
                    // Correction of offsets:
                    const string sect =
                        string("OFFSET_") + g_it->second->sensorLabel;
                    P.x += memFil.read_double(sect, "x", 0);
                    P.y += memFil.read_double(sect, "y", 0);
                    P.z += memFil.read_double(sect, "z", 0);
 
                    XYZidxs[g_it->second->sensorLabel] =
                        k;  // Save index correspondence
 
                    // Create the correspondence:
                    corrs.push_back(TMatchingPair(
                        k, k,  // Indices
                        P.x, P.y, P.z,  // "This"/Global coords
                        g_it->second->sensorPose.x(),
                        g_it->second->sensorPose.y(),
                        g_it->second->sensorPose
                            .z()  // "other"/local coordinates
                        ));
 
                    X[k] = P.x;
                    Y[k] = P.y;
                    Z[k] = P.z;
                }
 
                if (doConsistencyCheck && GPS.size() == 3)
                {
                    // XYZ[k] have the k'd final coordinates of each GPS
                    // GPS[k] are the CObservations:
 
                    // Compute the inter-GPS square distances:
                    CVectorDouble iGPSdist2(3);
 
                    // [0]: sq dist between:
                    // D_cov_rev_indexes[0],D_cov_rev_indexes[1]
                    TPoint3D P0(
                        X[XYZidxs[D_cov_rev_indexes[0]]],
                        Y[XYZidxs[D_cov_rev_indexes[0]]],
                        Z[XYZidxs[D_cov_rev_indexes[0]]]);
                    TPoint3D P1(
                        X[XYZidxs[D_cov_rev_indexes[1]]],
                        Y[XYZidxs[D_cov_rev_indexes[1]]],
                        Z[XYZidxs[D_cov_rev_indexes[1]]]);
                    TPoint3D P2(
                        X[XYZidxs[D_cov_rev_indexes[2]]],
                        Y[XYZidxs[D_cov_rev_indexes[2]]],
                        Z[XYZidxs[D_cov_rev_indexes[2]]]);
 
                    iGPSdist2[0] = P0.sqrDistanceTo(P1);
                    iGPSdist2[1] = P0.sqrDistanceTo(P2);
                    iGPSdist2[2] = P1.sqrDistanceTo(P2);
 
                    double mahaD = mrpt::math::mahalanobisDistance(
                        iGPSdist2, D_mean, D_cov_1);
                    outInfoTemp.mahalabis_quality_measure[i->first] = mahaD;
 
                    // cout << "x: " << iGPSdist2 << " MU: " <<  D_mean << " ->
                    // " << mahaD  << endl;
                }  // end consistency
 
                // Use a 6D matching method to estimate the location of the
                // vehicle:
                CPose3DQuat optimal_pose;
                double optimal_scale;
 
                // "this" (reference map) -> GPS global coordinates
                // "other" -> GPS local coordinates on the vehicle
                mrpt::tfest::se3_l2(
                    corrs, optimal_pose, optimal_scale, true);  // Force scale=1
                // cout << "optimal pose: " << optimal_pose << " " <<
                // optimal_scale << endl;
                MRPT_CHECK_NORMAL_NUMBER(optimal_pose.x());
                MRPT_CHECK_NORMAL_NUMBER(optimal_pose.y());
                MRPT_CHECK_NORMAL_NUMBER(optimal_pose.z());
                MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().x());
                MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().y());
                MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().z());
                MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().r());
 
                // Final vehicle pose:
                const CPose3D veh_pose = CPose3D(optimal_pose);
 
                // Add to the interpolator:
                robot_path.insert(i->first, veh_pose);
 
                // If we have W_star, compute the pose uncertainty:
                if (doUncertaintyCovs)
                {
                    CPose3DPDFGaussian final_veh_uncert;
                    final_veh_uncert.mean.setFromValues(0, 0, 0, 0, 0, 0);
                    final_veh_uncert.cov = outInfoTemp.W_star;
 
                    // Rotate the covariance according to the real vehicle pose:
                    final_veh_uncert.changeCoordinatesReference(veh_pose);
 
                    outInfoTemp.vehicle_uncertainty[i->first] =
                        final_veh_uncert.cov;
                }
            }
 
            // Show progress:
            if ((count++ % 100) == 0)
            {
#if MRPT_HAS_WXWIDGETS
                if (progDia3)
                {
                    if (!progDia3->Update(idx_in_GPSs)) abort = true;
                    wxTheApp->Yield();
                }
#endif
            }
        }  // end for i
 
#if MRPT_HAS_WXWIDGETS
        if (progDia3)
        {
            delete progDia3;
            progDia3 = nullptr;
        }
#endif
 
        if (PATH_SMOOTH_FILTER > 0 && robot_path.size() > 1)
        {
            CPose3DInterpolator filtered_robot_path = robot_path;
 
            // Do Angles smoother filter of the path:
            // ---------------------------------------------
            const double MAX_DIST_TO_FILTER = 4.0;
 
            for (auto i = robot_path.begin(); i != robot_path.end(); ++i)
            {
                mrpt::math::TPose3D p = i->second;
 
                CVectorDouble pitchs, rolls;  // The elements to average
 
                pitchs.push_back(p.pitch);
                rolls.push_back(p.roll);
 
                CPose3DInterpolator::iterator q = i;
                for (int k = 0;
                     k < PATH_SMOOTH_FILTER && q != robot_path.begin(); k++)
                {
                    --q;
                    if (abs(mrpt::system::timeDifference(q->first, i->first)) <
                        MAX_DIST_TO_FILTER)
                    {
                        pitchs.push_back(q->second.pitch);
                        rolls.push_back(q->second.roll);
                    }
                }
                q = i;
                for (int k = 0;
                     k < PATH_SMOOTH_FILTER && q != (--robot_path.end()); k++)
                {
                    ++q;
                    if (abs(mrpt::system::timeDifference(q->first, i->first)) <
                        MAX_DIST_TO_FILTER)
                    {
                        pitchs.push_back(q->second.pitch);
                        rolls.push_back(q->second.roll);
                    }
                }
 
                p.pitch = mrpt::math::averageWrap2Pi(pitchs);
                p.roll = mrpt::math::averageWrap2Pi(rolls);
 
                // save in filtered path:
                filtered_robot_path.insert(i->first, p);
            }
            // Replace:
            robot_path = filtered_robot_path;
 
        }  // end PATH_SMOOTH_FILTER
 
    }  // end step generate 6D path
 
    // Here we can set best_gps_path  (that with the max. number of RTK
    // fixed/foat readings):
    if (outInfo)
    {
        string bestLabel;
        size_t bestNum = 0;
        for (map<string, size_t>::iterator i = GPS_RTK_reads.begin();
             i != GPS_RTK_reads.end(); ++i)
        {
            if (i->second > bestNum)
            {
                bestNum = i->second;
                bestLabel = i->first;
            }
        }
        outInfoTemp.best_gps_path = gps_paths[bestLabel];
 
        // and transform to XYZ:
        // Correction of offsets:
        const string sect = string("OFFSET_") + bestLabel;
        const double off_X = memFil.read_double(sect, "x", 0);
        const double off_Y = memFil.read_double(sect, "y", 0);
        const double off_Z = memFil.read_double(sect, "z", 0);
 
        // map<TTimeStamp,TPoint3D> best_gps_path;      // time -> 3D local
        // coords
        for (map<Clock::time_point, TPoint3D>::iterator i =
                 outInfoTemp.best_gps_path.begin();
             i != outInfoTemp.best_gps_path.end(); ++i)
        {
            TPoint3D P;
            TPoint3D& pl = i->second;
            mrpt::topography::geodeticToENU_WGS84(
                TGeodeticCoords(
                    pl.x, pl.y, pl.z),  // i->second.x,i->second.y,i->second.z,
                // // lat, lon, heigh
                P,  // X Y Z
                ref);
 
            pl.x = P.x + off_X;
            pl.y = P.y + off_Y;
            pl.z = P.z + off_Z;
        }
    }  // end best_gps_path
 
    if (outInfo) *outInfo = outInfoTemp;
 
    MRPT_END
}