00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/probability_values.h"
00018
00019 #include "gtest/gtest.h"
00020
00021 namespace cartographer {
00022 namespace mapping {
00023 namespace {
00024
00025 TEST(ProbabilityValuesTest, OddsConversions) {
00026 EXPECT_NEAR(ProbabilityFromOdds(Odds(kMinProbability)), kMinProbability,
00027 1e-6);
00028 EXPECT_NEAR(ProbabilityFromOdds(Odds(kMaxProbability)), kMaxProbability,
00029 1e-6);
00030 EXPECT_NEAR(ProbabilityFromOdds(Odds(0.5)), 0.5, 1e-6);
00031 }
00032
00033 TEST(ProbabilityValuesTest, OddsConversionsCorrespondenceCost) {
00034 EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(Odds(
00035 CorrespondenceCostToProbability(kMaxCorrespondenceCost)))),
00036 kMaxCorrespondenceCost, 1e-6);
00037 EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(Odds(
00038 CorrespondenceCostToProbability(kMinCorrespondenceCost)))),
00039 kMinCorrespondenceCost, 1e-6);
00040 EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
00041 Odds(CorrespondenceCostToProbability(0.5)))),
00042 0.5, 1e-6);
00043 }
00044
00045 TEST(ProbabilityValuesTest,
00046 ProbabilityValueToCorrespondenceCostValueConversions) {
00047 for (uint16 i = 0; i < 32768; ++i) {
00048 EXPECT_EQ(ProbabilityValueToCorrespondenceCostValue(
00049 CorrespondenceCostValueToProbabilityValue(i)),
00050 i);
00051 EXPECT_EQ(CorrespondenceCostValueToProbabilityValue(
00052 ProbabilityValueToCorrespondenceCostValue(i)),
00053 i);
00054 }
00055 }
00056
00057 TEST(ProbabilityValuesTest,
00058 ProbabilityValueToCorrespondenceCostValueConversionsWithUpdateMarker) {
00059 for (uint16 i = 1; i < 32768; ++i) {
00060 EXPECT_EQ(ProbabilityValueToCorrespondenceCostValue(
00061 CorrespondenceCostValueToProbabilityValue(i + kUpdateMarker)),
00062 i + kUpdateMarker);
00063 EXPECT_EQ(CorrespondenceCostValueToProbabilityValue(
00064 ProbabilityValueToCorrespondenceCostValue(i + kUpdateMarker)),
00065 i + kUpdateMarker);
00066 }
00067 }
00068
00069 TEST(ProbabilityValuesTest, ConversionLookUpTable) {
00070 EXPECT_NEAR(ValueToProbability(0), 1.f - ValueToCorrespondenceCost(0), 1e-6);
00071 for (uint16 i = 1; i < 32768; ++i) {
00072 EXPECT_NEAR(ValueToProbability(i), ValueToCorrespondenceCost(i), 1e-6)
00073 << " i " << i;
00074 }
00075 }
00076
00077 TEST(ProbabilityValuesTest, CellUpdate) {
00078 std::vector<uint16> probability_table =
00079 ComputeLookupTableToApplyOdds(Odds(0.9f));
00080 std::vector<uint16> correspondence_table =
00081 ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9f));
00082 uint16 cell_pg_pre_update = 0;
00083 uint16 cell_cg_pre_update = 0;
00084 uint16 cell_pg_post_update = probability_table[cell_pg_pre_update];
00085 uint16 cell_cg_post_update = correspondence_table[cell_cg_pre_update];
00086 float p_post = ValueToProbability(cell_pg_post_update);
00087 float c_post = ValueToCorrespondenceCost(cell_cg_post_update);
00088 EXPECT_NEAR(p_post, 1.f - c_post, 1e-6);
00089 int num_evaluations = 5000;
00090 for (int i_probability = 0; i_probability < num_evaluations;
00091 ++i_probability) {
00092 float p = (static_cast<float>(i_probability) /
00093 static_cast<float>(num_evaluations)) *
00094 (kMaxProbability - kMinProbability) +
00095 kMinProbability;
00096 cell_pg_pre_update = ProbabilityToValue(p);
00097 cell_cg_pre_update =
00098 CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(p));
00099 float p_value =
00100 (common::Clamp(p, kMinProbability, kMaxProbability) - kMinProbability) *
00101 (32766.f / (kMaxProbability - kMinProbability));
00102 float cvalue = (common::Clamp(ProbabilityToCorrespondenceCost(p),
00103 kMinProbability, kMaxProbability) -
00104 kMinProbability) *
00105 (32766.f / (kMaxProbability - kMinProbability));
00106
00107 EXPECT_NEAR(cell_pg_pre_update, 32768 - cell_cg_pre_update, 1)
00108 << "p " << p << " p_value " << p_value << " cvalue " << cvalue
00109 << " p_valuei " << common::RoundToInt(p_value) << " cvaluei "
00110 << common::RoundToInt(cvalue);
00111 cell_pg_post_update = probability_table[cell_pg_pre_update];
00112 cell_cg_post_update = correspondence_table[cell_cg_pre_update];
00113 p_post = ValueToProbability(cell_pg_post_update);
00114 c_post = ValueToCorrespondenceCost(cell_cg_post_update);
00115 EXPECT_NEAR(p_post, 1.f - c_post, 5e-5)
00116 << "p " << p << " " << p_post - 1.f + c_post;
00117 }
00118 }
00119
00120 TEST(ProbabilityValuesTest, MultipleCellUpdate) {
00121 std::vector<uint16> probability_table =
00122 ComputeLookupTableToApplyOdds(Odds(0.55f));
00123 std::vector<uint16> correspondence_table =
00124 ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.55f));
00125 uint16 cell_pg_post_update = probability_table[0];
00126 uint16 cell_cg_post_update = correspondence_table[0];
00127 float p_post = ValueToProbability(cell_pg_post_update);
00128 float c_post = ValueToCorrespondenceCost(cell_cg_post_update);
00129 EXPECT_NEAR(p_post, 1.f - c_post, 1e-6);
00130 int num_evaluations = 5000;
00131 for (int i_probability = 0; i_probability < num_evaluations;
00132 ++i_probability) {
00133 float p = (static_cast<float>(i_probability) /
00134 static_cast<float>(num_evaluations)) *
00135 (kMaxProbability - kMinProbability) +
00136 kMinProbability;
00137 cell_pg_post_update = ProbabilityToValue(p) + kUpdateMarker;
00138 cell_cg_post_update =
00139 CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(p)) +
00140 kUpdateMarker;
00141 for (int i_update = 0; i_update < 20; ++i_update) {
00142 cell_pg_post_update =
00143 probability_table[cell_pg_post_update - kUpdateMarker];
00144 cell_cg_post_update =
00145 correspondence_table[cell_cg_post_update - kUpdateMarker];
00146 }
00147 p_post = ValueToProbability(cell_pg_post_update);
00148 c_post = ValueToCorrespondenceCost(cell_cg_post_update);
00149 EXPECT_NEAR(p_post, 1.f - c_post, 5e-5)
00150 << "p " << p << " p_post " << p_post << " " << p_post - 1.f + c_post;
00151 }
00152 }
00153
00154 TEST(ProbabilityValuesTest, EqualityLookupTableToApplyOdds) {
00155 std::vector<uint16> probability_table = ComputeLookupTableToApplyOdds(0.3);
00156 std::vector<uint16> correspondence_table =
00157 ComputeLookupTableToApplyCorrespondenceCostOdds(0.3);
00158 for (int i = 0; i < 32768; ++i) {
00159 EXPECT_NEAR(
00160 probability_table[i],
00161 CorrespondenceCostValueToProbabilityValue(
00162 correspondence_table[ProbabilityValueToCorrespondenceCostValue(i)]),
00163 1);
00164 EXPECT_NEAR(
00165 ProbabilityValueToCorrespondenceCostValue(
00166 probability_table[CorrespondenceCostValueToProbabilityValue(i)]),
00167 correspondence_table[i], 1);
00168 }
00169 }
00170
00171 }
00172 }
00173 }