common.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
         |                     Mobile Robot Programming Toolkit (MRPT)               |
         |                          http://www.mrpt.org/                             |
         |                                                                           |
         | Copyright (c) 2005-2016, 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/math/utils.h>
#include "mrpt_graphslam_2d/misc/common.h"

#include <mrpt/version.h>
#if MRPT_VERSION>=0x199
#include <mrpt/containers/stl_containers_utils.h>
using namespace mrpt::containers;
#else
using namespace mrpt::utils;
#endif

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

std::string mrpt::graphslam::detail::getGridMapAlignmentResultsAsString(
                        const mrpt::poses::CPosePDF& pdf,
                        const mrpt::slam::CGridMapAligner::TReturnInfo& ret_info) {

                        CPosePDFSOG::Ptr pdf_out = CPosePDFSOG::Create();
                        pdf_out->copyFrom(pdf);
                        CPose2D pose_out; CMatrixDouble33 cov_out;
                        pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out);

                        stringstream ss;
                        ss << "--------------------" << endl;
                        ss << "Results: " << endl
                                << "\tPDFPtr pose: " << pdf.getMeanVal() << endl
                                << "\t# Correspondences: " << ret_info.correspondences.size() << endl
                                << "\tAlignment goodness: " << ret_info.goodness << endl
                                << "\tModes size: " << pdf_out->size() << endl
                                << "\tModes: " << endl << getSTLContainerAsString(pdf_out->getSOGModes())
                                << "\tMost likely pose: " << pose_out << endl
                                << "\tCorresponding covariance matrix: " << endl
                                << cov_out << endl;
                        ss << "--------------------" << endl;
                        return ss.str();
}

bool mrpt::graphslam::detail::isEssentiallyZero(
                const mrpt::poses::CPose2D& p) {
        double epsilon = 0.001;
        return (
                        approximatelyEqual(p.x(), 0.0, epsilon) && // all 0s
                        approximatelyEqual(p.y(), 0.0, epsilon) &&
                        approximatelyEqual(p.phi(), 0.0, epsilon));
}