common.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include <mrpt/math/utils.h>
12 
13 #include <mrpt/version.h>
14 #if MRPT_VERSION>=0x199
15 #include <mrpt/containers/stl_containers_utils.h>
16 using namespace mrpt::containers;
17 #else
18 using namespace mrpt::utils;
19 #endif
20 
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
23 using namespace std;
24 using namespace mrpt::math;
25 
27  const mrpt::poses::CPosePDF& pdf,
29 
30  CPosePDFSOG::Ptr pdf_out = CPosePDFSOG::Create();
31  pdf_out->copyFrom(pdf);
32  CPose2D pose_out; CMatrixDouble33 cov_out;
33  pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out);
34 
35  stringstream ss;
36  ss << "--------------------" << endl;
37  ss << "Results: " << endl
38  << "\tPDFPtr pose: " << pdf.getMeanVal() << endl
39  << "\t# Correspondences: " << ret_info.correspondences.size() << endl
40  << "\tAlignment goodness: " << ret_info.goodness << endl
41  << "\tModes size: " << pdf_out->size() << endl
42  << "\tModes: " << endl << getSTLContainerAsString(pdf_out->getSOGModes())
43  << "\tMost likely pose: " << pose_out << endl
44  << "\tCorresponding covariance matrix: " << endl
45  << cov_out << endl;
46  ss << "--------------------" << endl;
47  return ss.str();
48 }
49 
51  const mrpt::poses::CPose2D& p) {
52  double epsilon = 0.001;
53  return (
54  approximatelyEqual(p.x(), 0.0, epsilon) && // all 0s
55  approximatelyEqual(p.y(), 0.0, epsilon) &&
56  approximatelyEqual(p.phi(), 0.0, epsilon));
57 }
double epsilon
std::string getSTLContainerAsString(const T &t)
std::string getGridMapAlignmentResultsAsString(const mrpt::poses::CPosePDF &pdf, const mrpt::slam::CGridMapAligner::TReturnInfo &ret_info)
Definition: common.cpp:26
const double & phi() const
GLfloat GLfloat p
bool isEssentiallyZero(const mrpt::poses::CPose2D &p)
Definition: common.cpp:50
string ss
mrpt::utils::TMatchingPairList correspondences
bool approximatelyEqual(T1 a, T1 b, T2 epsilon)


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Sat May 2 2020 03:44:17