From 00f8d902236dabed0c85932016c89555b08097c8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Dec 2021 10:30:02 -0500 Subject: [PATCH 01/62] Fix ambiguity --- gtsam/nonlinear/NonlinearFactorGraph.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 2fad561be0..84b1516e95 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -269,10 +269,10 @@ namespace gtsam { dot(os, values, keyFormatter, graphvizFormatting); } /** \deprecated */ - void GTSAM_DEPRECATED saveGraph( - const std::string& filename, const Values& values, - const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void GTSAM_DEPRECATED + saveGraph(const std::string& filename, const Values& values, + const GraphvizFormatting& graphvizFormatting, + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { saveGraph(filename, values, keyFormatter, graphvizFormatting); } #endif From 92744d13c63c417735ef6f1e86a75571b32c71f5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 Dec 2021 10:30:13 -0500 Subject: [PATCH 02/62] Add auto --- examples/UGM_small.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index f4f3f1fd0b..3829a5c917 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -50,8 +50,7 @@ int main(int argc, char** argv) { // Print the UGM distribution cout << "\nUGM distribution:" << endl; - vector allPosbValues = cartesianProduct( - Cathy & Heather & Mark & Allison); + auto allPosbValues = cartesianProduct(Cathy & Heather & Mark & Allison); for (size_t i = 0; i < allPosbValues.size(); ++i) { DiscreteFactor::Values values = allPosbValues[i]; double prodPot = graph(values); From fa38b297afd5ef9824389cf3b0abb12b09e3c497 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 30 Dec 2021 12:50:26 -0500 Subject: [PATCH 03/62] forece nonnegative scale for Sim(3) --- gtsam/geometry/Similarity3.cpp | 4 +- python/gtsam/tests/test_Sim3.py | 582 ++++++++++++++++++++++++++++++++ 2 files changed, 585 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index ea7a6d0498..e8d6e75106 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -40,6 +40,8 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, } /// Form inner products x and y and calculate scale. +// We force the scale to be a non-negative quantity +// (see Section 10.1 of https://ethaneade.com/lie_groups.pdf) static double calculateScale(const Point3Pairs &d_abPointPairs, const Rot3 &aRb) { double x = 0, y = 0; @@ -50,7 +52,7 @@ static double calculateScale(const Point3Pairs &d_abPointPairs, y += da.transpose() * da_prime; x += da_prime.transpose() * da_prime; } - const double s = y / x; + const double s = std::fabs(y / x); return s; } diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index 001321e2cd..c00a36435e 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -10,6 +10,7 @@ """ # pylint: disable=no-name-in-module import unittest +from typing import List, Optional import numpy as np @@ -129,6 +130,587 @@ def test_align_poses_scaled_squares(self): for aTi, bTi in zip(aTi_list, bTi_list): self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + def test_align_via_Sim3_to_poses_skydio32(self) -> None: + """Ensure scale estimate of Sim(3) object is non-negative. + + Comes from real data (from Skydio-32 Crane Mast sequence with a SIFT front-end). + """ + poses_gt = [ + Pose3( + Rot3( + [ + [0.696305769, -0.0106830792, -0.717665705], + [0.00546412488, 0.999939148, -0.00958346857], + [0.717724415, 0.00275160848, 0.696321772], + ] + ), + Point3(5.83077801, -0.94815149, 0.397751679), + ), + Pose3( + Rot3( + [ + [0.692272397, -0.00529704529, -0.721616549], + [0.00634689669, 0.999979075, -0.00125157022], + [0.721608079, -0.0037136016, 0.692291531], + ] + ), + Point3(5.03853323, -0.97547405, -0.348177392), + ), + Pose3( + Rot3( + [ + [0.945991981, -0.00633548292, -0.324128225], + [0.00450436485, 0.999969379, -0.00639931046], + [0.324158843, 0.00459370582, 0.945991552], + ] + ), + Point3(4.13186176, -0.956364218, -0.796029527), + ), + Pose3( + Rot3( + [ + [0.999553623, -0.00346470207, -0.0296740626], + [0.00346104216, 0.999993995, -0.00017469881], + [0.0296744897, 7.19175654e-05, 0.999559612], + ] + ), + Point3(3.1113898, -0.928583423, -0.90539337), + ), + Pose3( + Rot3( + [ + [0.967850252, -0.00144846042, 0.251522892], + [0.000254511591, 0.999988546, 0.00477934325], + [-0.251526934, -0.00456167299, 0.967839535], + ] + ), + Point3(2.10584013, -0.921303194, -0.809322971), + ), + Pose3( + Rot3( + [ + [0.969854065, 0.000629052774, 0.243685716], + [0.000387180179, 0.999991428, -0.00412234326], + [-0.243686221, 0.00409242166, 0.969845508], + ] + ), + Point3(1.0753788, -0.913035975, -0.616584091), + ), + Pose3( + Rot3( + [ + [0.998189342, 0.00110235337, 0.0601400045], + [-0.00110890447, 0.999999382, 7.55559042e-05], + [-0.060139884, -0.000142108649, 0.998189948], + ] + ), + Point3(0.029993558, -0.951495122, -0.425525143), + ), + Pose3( + Rot3( + [ + [0.999999996, -2.62868666e-05, -8.67178281e-05], + [2.62791334e-05, 0.999999996, -8.91767396e-05], + [8.67201719e-05, 8.91744604e-05, 0.999999992], + ] + ), + Point3(-0.973569417, -0.936340994, -0.253464928), + ), + Pose3( + Rot3( + [ + [0.99481227, -0.00153645011, 0.101716252], + [0.000916919443, 0.999980747, 0.00613725239], + [-0.101723724, -0.00601214847, 0.994794525], + ] + ), + Point3(-2.02071256, -0.955446292, -0.240707879), + ), + Pose3( + Rot3( + [ + [0.89795602, -0.00978591184, 0.43997636], + [0.00645921401, 0.999938116, 0.00905779513], + [-0.440037771, -0.00529159974, 0.89796366], + ] + ), + Point3(-2.94096695, -0.939974858, 0.0934225593), + ), + Pose3( + Rot3( + [ + [0.726299119, -0.00916784876, 0.687318077], + [0.00892018672, 0.999952563, 0.0039118575], + [-0.687321336, 0.00328981905, 0.726346444], + ] + ), + Point3(-3.72843416, -0.897889251, 0.685129502), + ), + Pose3( + Rot3( + [ + [0.506756029, -0.000331706105, 0.862089858], + [0.00613841257, 0.999975964, -0.00322354286], + [-0.862068067, 0.00692541035, 0.506745885], + ] + ), + Point3(-4.3909926, -0.890883291, 1.43029524), + ), + Pose3( + Rot3( + [ + [0.129316352, -0.00206958814, 0.991601896], + [0.00515932597, 0.999985691, 0.00141424797], + [-0.991590634, 0.00493310721, 0.129325179], + ] + ), + Point3(-4.58510846, -0.922534227, 2.36884523), + ), + Pose3( + Rot3( + [ + [0.599853194, -0.00890004681, -0.800060263], + [0.00313716318, 0.999956608, -0.00877161373], + [0.800103615, 0.00275175707, 0.599855085], + ] + ), + Point3(5.71559638, 0.486863076, 0.279141372), + ), + Pose3( + Rot3( + [ + [0.762552447, 0.000836438681, -0.646926069], + [0.00211337894, 0.999990607, 0.00378404105], + [0.646923157, -0.00425272942, 0.762543517], + ] + ), + Point3(5.00243443, 0.513321893, -0.466921769), + ), + Pose3( + Rot3( + [ + [0.930381645, -0.00340164355, -0.36657678], + [0.00425636616, 0.999989781, 0.00152338305], + [0.366567852, -0.00297761145, 0.930386617], + ] + ), + Point3(4.05404984, 0.493385291, -0.827904571), + ), + Pose3( + Rot3( + [ + [0.999996073, -0.00278379707, -0.000323508543], + [0.00278790921, 0.999905063, 0.0134941517], + [0.000285912831, -0.0134950006, 0.999908897], + ] + ), + Point3(3.04724478, 0.491451306, -0.989571061), + ), + Pose3( + Rot3( + [ + [0.968578343, -0.002544616, 0.248695527], + [0.000806130148, 0.999974526, 0.00709200332], + [-0.248707238, -0.0066686795, 0.968555721], + ] + ), + Point3(2.05737869, 0.46840177, -0.546344594), + ), + Pose3( + Rot3( + [ + [0.968827882, 0.000182770584, 0.247734722], + [-0.000558107079, 0.9999988, 0.00144484904], + [-0.24773416, -0.00153807255, 0.968826821], + ] + ), + Point3(1.14019947, 0.469674641, -0.0491053805), + ), + Pose3( + Rot3( + [ + [0.991647805, 0.00197867892, 0.128960146], + [-0.00247518407, 0.999990129, 0.00368991165], + [-0.128951572, -0.00397829284, 0.991642914], + ] + ), + Point3(0.150270471, 0.457867448, 0.103628642), + ), + Pose3( + Rot3( + [ + [0.992244594, 0.00477781876, -0.124208847], + [-0.0037682125, 0.999957938, 0.00836195891], + [0.124243574, -0.00782906317, 0.992220862], + ] + ), + Point3(-0.937954641, 0.440532658, 0.154265069), + ), + Pose3( + Rot3( + [ + [0.999591078, 0.00215462857, -0.0285137564], + [-0.00183807224, 0.999936443, 0.0111234301], + [0.028535911, -0.0110664711, 0.999531507], + ] + ), + Point3(-1.95622231, 0.448914367, -0.0859439782), + ), + Pose3( + Rot3( + [ + [0.931835342, 0.000956922238, 0.362880212], + [0.000941640753, 0.99998678, -0.00505501434], + [-0.362880252, 0.00505214382, 0.931822122], + ] + ), + Point3(-2.85557418, 0.434739285, 0.0793777177), + ), + Pose3( + Rot3( + [ + [0.781615218, -0.0109886966, 0.623664238], + [0.00516954657, 0.999924591, 0.011139446], + [-0.623739616, -0.00548270158, 0.781613084], + ] + ), + Point3(-3.67524552, 0.444074681, 0.583718622), + ), + Pose3( + Rot3( + [ + [0.521291761, 0.00264805046, 0.853374051], + [0.00659087718, 0.999952868, -0.00712898365], + [-0.853352707, 0.00934076542, 0.521249738], + ] + ), + Point3(-4.35541796, 0.413479707, 1.31179007), + ), + Pose3( + Rot3( + [ + [0.320164205, -0.00890839482, 0.947319884], + [0.00458409304, 0.999958649, 0.007854118], + [-0.947350678, 0.00182799903, 0.320191803], + ] + ), + Point3(-4.71617526, 0.476674479, 2.16502998), + ), + Pose3( + Rot3( + [ + [0.464861609, 0.0268597443, -0.884976415], + [-0.00947397841, 0.999633409, 0.0253631906], + [0.885333239, -0.00340614699, 0.464945663], + ] + ), + Point3(6.11772094, 1.63029238, 0.491786626), + ), + Pose3( + Rot3( + [ + [0.691647251, 0.0216006293, -0.721912024], + [-0.0093228132, 0.999736395, 0.020981541], + [0.722174939, -0.00778156302, 0.691666308], + ] + ), + Point3(5.46912979, 1.68759322, -0.288499782), + ), + Pose3( + Rot3( + [ + [0.921208931, 0.00622640471, -0.389018433], + [-0.00686296262, 0.999976419, -0.000246683913], + [0.389007724, 0.00289706631, 0.92122994], + ] + ), + Point3(4.70156942, 1.72186229, -0.806181015), + ), + Pose3( + Rot3( + [ + [0.822397705, 0.00276497594, 0.568906142], + [0.00804891535, 0.999831556, -0.016494662], + [-0.568855921, 0.0181442503, 0.822236923], + ] + ), + Point3(-3.51368714, 1.59619714, 0.437437437), + ), + Pose3( + Rot3( + [ + [0.726822937, -0.00545541524, 0.686803193], + [0.00913794245, 0.999956756, -0.00172754968], + [-0.686764068, 0.00753159111, 0.726841357], + ] + ), + Point3(-4.29737821, 1.61462527, 1.11537749), + ), + Pose3( + Rot3( + [ + [0.402595481, 0.00697612855, 0.915351441], + [0.0114113638, 0.999855006, -0.0126391687], + [-0.915306892, 0.0155338804, 0.4024575], + ] + ), + Point3(-4.6516433, 1.6323107, 1.96579585), + ), + ] + # from estimated cameras + unaligned_pose_dict = { + 2: Pose3( + Rot3( + [ + [-0.681949, -0.568276, 0.460444], + [0.572389, -0.0227514, 0.819667], + [-0.455321, 0.822524, 0.34079], + ] + ), + Point3(-1.52189, 0.78906, -1.60608), + ), + 4: Pose3( + Rot3( + [ + [-0.817805393, -0.575044816, 0.022755196], + [0.0478829397, -0.0285875849, 0.998443776], + [-0.573499401, 0.81762229, 0.0509139174], + ] + ), + Point3(-1.22653168, 0.686485651, -1.39294168), + ), + 3: Pose3( + Rot3( + [ + [-0.783051568, -0.571905041, 0.244448085], + [0.314861464, -0.0255673164, 0.948793218], + [-0.536369743, 0.819921299, 0.200091385], + ] + ), + Point3(-1.37620079, 0.721408674, -1.49945316), + ), + 5: Pose3( + Rot3( + [ + [-0.818916586, -0.572896131, 0.0341415873], + [0.0550548476, -0.0192038786, 0.99829864], + [-0.571265778, 0.819402974, 0.0472670839], + ] + ), + Point3(-1.06370243, 0.663084159, -1.27672831), + ), + 6: Pose3( + Rot3( + [ + [-0.798825521, -0.571995242, 0.186277293], + [0.243311017, -0.0240196245, 0.969650869], + [-0.550161372, 0.819905178, 0.158360233], + ] + ), + Point3(-0.896250742, 0.640768239, -1.16984756), + ), + 7: Pose3( + Rot3( + [ + [-0.786416666, -0.570215296, 0.237493882], + [0.305475635, -0.0248440676, 0.951875732], + [-0.536873788, 0.821119534, 0.193724669], + ] + ), + Point3(-0.740385043, 0.613956842, -1.05908579), + ), + 8: Pose3( + Rot3( + [ + [-0.806252832, -0.57019757, 0.157578877], + [0.211046715, -0.0283979846, 0.977063375], + [-0.55264424, 0.821016617, 0.143234279], + ] + ), + Point3(-0.58333517, 0.549832698, -0.9542864), + ), + 9: Pose3( + Rot3( + [ + [-0.821191354, -0.557772774, -0.120558255], + [-0.125347331, -0.0297958331, 0.991665395], + [-0.556716092, 0.829458703, -0.0454472483], + ] + ), + Point3(-0.436483039, 0.55003923, -0.850733187), + ), + 21: Pose3( + Rot3( + [ + [-0.778607603, -0.575075476, 0.251114312], + [0.334920968, -0.0424301164, 0.941290407], + [-0.53065822, 0.816999316, 0.225641247], + ] + ), + Point3(-0.736735967, 0.571415247, -0.738663611), + ), + 17: Pose3( + Rot3( + [ + [-0.818569806, -0.573904529, 0.0240221722], + [0.0512889176, -0.0313725422, 0.998190969], + [-0.572112681, 0.818321059, 0.0551155579], + ] + ), + Point3(-1.36150982, 0.724829031, -1.16055631), + ), + 18: Pose3( + Rot3( + [ + [-0.812668105, -0.582027424, 0.0285417146], + [0.0570298244, -0.0306936169, 0.997900547], + [-0.579929436, 0.812589675, 0.0581366453], + ] + ), + Point3(-1.20484771, 0.762370343, -1.05057127), + ), + 20: Pose3( + Rot3( + [ + [-0.748446406, -0.580905382, 0.319963926], + [0.416860654, -0.0368374152, 0.908223651], + [-0.515805363, 0.813137099, 0.269727429], + ] + ), + Point3(569.449421, -907.892555, -794.585647), + ), + 22: Pose3( + Rot3( + [ + [-0.826878177, -0.559495019, -0.0569017041], + [-0.0452256802, -0.0346974602, 0.99837404], + [-0.560559647, 0.828107125, 0.00338702978], + ] + ), + Point3(-0.591431172, 0.55422253, -0.654656597), + ), + 29: Pose3( + Rot3( + [ + [-0.785759779, -0.574532433, -0.229115805], + [-0.246020939, -0.049553424, 0.967996981], + [-0.567499134, 0.81698038, -0.102409921], + ] + ), + Point3(69.4916073, 240.595227, -493.278045), + ), + 23: Pose3( + Rot3( + [ + [-0.783524382, -0.548569702, -0.291823276], + [-0.316457553, -0.051878563, 0.94718701], + [-0.534737468, 0.834493797, -0.132950906], + ] + ), + Point3(-5.93496204, 41.9304933, -3.06881633), + ), + 10: Pose3( + Rot3( + [ + [-0.766833992, -0.537641809, -0.350580824], + [-0.389506676, -0.0443270797, 0.919956336], + [-0.510147213, 0.84200736, -0.175423563], + ] + ), + Point3(234.185458, 326.007989, -691.769777), + ), + 30: Pose3( + Rot3( + [ + [-0.754844165, -0.559278755, -0.342662459], + [-0.375790683, -0.0594160018, 0.92479787], + [-0.537579435, 0.826847636, -0.165321923], + ] + ), + Point3(-5.93398168, 41.9107972, -3.07385081), + ), + } + + unaligned_pose_list = [] + for i in range(32): + wTi = unaligned_pose_dict.get(i, None) + unaligned_pose_list.append(wTi) + # GT poses are the reference/target + rSe = align_poses_sim3_ignore_missing(aTi_list=poses_gt, bTi_list=unaligned_pose_list) + assert rSe.scale() >= 0 + + +def align_poses_sim3_ignore_missing(aTi_list: List[Optional[Pose3]], bTi_list: List[Optional[Pose3]]) -> Similarity3: + """Align by similarity transformation, but allow missing estimated poses in the input. + + Note: this is a wrapper for align_poses_sim3() that allows for missing poses/dropped cameras. + This is necessary, as align_poses_sim3() requires a valid pose for every input pair. + + We force SIM(3) alignment rather than SE(3) alignment. + We assume the two trajectories are of the exact same length. + + Args: + aTi_list: reference poses in frame "a" which are the targets for alignment + bTi_list: input poses which need to be aligned to frame "a" + + Returns: + aSb: Similarity(3) object that aligns the two pose graphs. + """ + assert len(aTi_list) == len(bTi_list) + + # only choose target poses for which there is a corresponding estimated pose + corresponding_aTi_list = [] + valid_camera_idxs = [] + valid_bTi_list = [] + for i, bTi in enumerate(bTi_list): + if bTi is not None: + valid_camera_idxs.append(i) + valid_bTi_list.append(bTi) + corresponding_aTi_list.append(aTi_list[i]) + + aSb = align_poses_sim3(aTi_list=corresponding_aTi_list, bTi_list=valid_bTi_list) + return aSb + + +def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity3: + """Align two pose graphs via similarity transformation. Note: poses cannot be missing/invalid. + + We force SIM(3) alignment rather than SE(3) alignment. + We assume the two trajectories are of the exact same length. + + Args: + aTi_list: reference poses in frame "a" which are the targets for alignment + bTi_list: input poses which need to be aligned to frame "a" + + Returns: + aSb: Similarity(3) object that aligns the two pose graphs. + """ + n_to_align = len(aTi_list) + assert len(aTi_list) == len(bTi_list) + assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames" + + ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) + + aSb = Similarity3.Align(ab_pairs) + + if np.isnan(aSb.scale()) or aSb.scale() == 0: + # we have run into a case where points have no translation between them (i.e. panorama). + # We will first align the rotations and then align the translation by using centroids. + # TODO: handle it in GTSAM + + # align the rotations first, so that we can find the translation between the two panoramas + aSb = Similarity3(aSb.rotation(), np.zeros((3,)), 1.0) + aTi_list_rot_aligned = [aSb.transformFrom(bTi) for bTi in bTi_list] + + # fit a single translation motion to the centroid + aTi_centroid = np.array([aTi.translation() for aTi in aTi_list]).mean(axis=0) + aTi_rot_aligned_centroid = np.array([aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0) + + # construct the final SIM3 transform + aSb = Similarity3(aSb.rotation(), aTi_centroid - aTi_rot_aligned_centroid, 1.0) + + return aSb + if __name__ == "__main__": unittest.main() From 09573a36c9017c0536c58b72791e79157c2c4702 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 31 Dec 2021 16:16:56 -0500 Subject: [PATCH 04/62] Moved empty to Factor base class. --- gtsam/discrete/DiscreteFactor.h | 3 --- gtsam/inference/Factor.h | 3 +++ gtsam/linear/GaussianFactor.h | 3 --- gtsam/linear/HessianFactor.h | 3 --- gtsam/linear/JacobianFactor.h | 3 --- gtsam/slam/RegularImplicitSchurFactor.h | 4 ---- gtsam/symbolic/SymbolicFactor.h | 3 --- 7 files changed, 3 insertions(+), 19 deletions(-) diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index d7deca3838..76ed703bb2 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -73,9 +73,6 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { Base::print(s, formatter); } - /** Test whether the factor is empty */ - virtual bool empty() const { return size() == 0; } - /// @} /// @name Standard Interface /// @{ diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index c0ea4ea78b..1ca97cea51 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -112,6 +112,9 @@ typedef FastSet FactorIndexSet; /// @name Standard Interface /// @{ + /// Whether the factor is empty (involves zero variables). + bool empty() const { return keys_.empty(); } + /// First key Key front() const { return keys_.front(); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 3347228684..672f5aa0d9 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -117,9 +117,6 @@ namespace gtsam { /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; - /** Test whether the factor is empty */ - virtual bool empty() const = 0; - /** * Construct the corresponding anti-factor to negate information * stored stored in this factor. diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 0f4c993fe3..7020d6edd5 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -221,9 +221,6 @@ namespace gtsam { */ GaussianFactor::shared_ptr negate() const override; - /** Check if the factor is empty. TODO: How should this be defined? */ - bool empty() const override { return size() == 0 /*|| rows() == 0*/; } - /** Return the constant term \f$ f \f$ as described above * @return The constant term \f$ f \f$ */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 4d4480d320..ddf614910c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -260,9 +260,6 @@ namespace gtsam { */ GaussianFactor::shared_ptr negate() const override; - /** Check if the factor is empty. TODO: How should this be defined? */ - bool empty() const override { return size() == 0 /*|| rows() == 0*/; } - /** is noise model constrained ? */ bool isConstrained() const { return model_ && model_->isConstrained(); diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index b4a3417191..340f84018d 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -260,10 +260,6 @@ class RegularImplicitSchurFactor: public GaussianFactor { "RegularImplicitSchurFactor::clone non implemented"); } - bool empty() const override { - return false; - } - GaussianFactor::shared_ptr negate() const override { return boost::make_shared >(keys_, FBlocks_, PointCovariance_, E_, b_); diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 2a488a4daf..767998d22c 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -144,9 +144,6 @@ namespace gtsam { /// @name Standard Interface /// @{ - /** Whether the factor is empty (involves zero variables). */ - bool empty() const { return keys_.empty(); } - /** Eliminate the variables in \c keys, in the order specified in \c keys, returning a * conditional and marginal. */ std::pair, boost::shared_ptr > From 55f31ab2d73e0b775caae61ecb826d054e92de5b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:38:20 -0500 Subject: [PATCH 05/62] Revive BetweenFactorEM, without LieVector --- gtsam_unstable/slam/BetweenFactorEM.h | 4 ++ .../slam/tests/testBetweenFactorEM.cpp | 52 +++++++++---------- 2 files changed, 30 insertions(+), 26 deletions(-) diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 572935da3f..9c19bae8c5 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -421,4 +421,8 @@ class BetweenFactorEM: public NonlinearFactor { }; // \class BetweenFactorEM +/// traits +template +struct traits > : public Testable > {}; + } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 4d6e1912a1..f43ae293ef 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include @@ -21,26 +22,24 @@ using namespace gtsam; // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below // to reenable the test. -#if 0 +// #if 0 /* ************************************************************************* */ -LieVector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM& factor){ +Vector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM& factor){ gtsam::Values values; values.insert(key1, p1); values.insert(key2, p2); - // LieVector err = factor.whitenedError(values); - // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ -LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor& factor){ +Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor& factor){ gtsam::Values values; values.insert(key1, p1); values.insert(key2, p2); - // LieVector err = factor.whitenedError(values); + // Vector err = factor.whitenedError(values); // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ @@ -99,8 +98,8 @@ TEST( BetweenFactorEM, EvaluateError) Vector actual_err_wh = f.whitenedError(values); - Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier, prior_inlier, prior_outlier); actual_err_wh = h_EM.whitenedError(values); - actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); @@ -178,7 +177,7 @@ TEST (BetweenFactorEM, jacobian ) { // compare to standard between factor BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); - Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); std::vector H_actual_stnd_unwh(2); (void)h.unwhitenedError(values, H_actual_stnd_unwh); @@ -190,12 +189,13 @@ TEST (BetweenFactorEM, jacobian ) { // CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); - Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); + using std::placeholders::_1; + Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); + Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); // try to check numerical derivatives of a standard between factor - Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); + Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // @@ -240,8 +240,8 @@ TEST( BetweenFactorEM, CaseStudy) Vector actual_err_unw = f.unwhitenedError(values); Vector actual_err_wh = f.whitenedError(values); - Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); if (debug){ cout << "p_inlier_outler: "<print("model_inlier:"); - model_outlier->print("model_outlier:"); - model_inlier_new->print("model_inlier_new:"); - model_outlier_new->print("model_outlier_new:"); + // model_inlier->print("model_inlier:"); + // model_outlier->print("model_outlier:"); + // model_inlier_new->print("model_inlier_new:"); + // model_outlier_new->print("model_outlier_new:"); } -#endif +// #endif /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} From 70092ca18aade8399f2173ba2c212eb2cc6c2eff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:43:48 -0500 Subject: [PATCH 06/62] Remove Deprecated Lie* classes --- Using-GTSAM-EXPORT.md | 2 +- gtsam/base/CMakeLists.txt | 3 - gtsam/base/LieMatrix.h | 26 ---- gtsam/base/LieScalar.h | 26 ---- gtsam/base/LieVector.h | 26 ---- gtsam/base/deprecated/LieMatrix.h | 152 -------------------- gtsam/base/deprecated/LieScalar.h | 88 ------------ gtsam/base/deprecated/LieVector.h | 121 ---------------- gtsam/base/tests/testLieMatrix.cpp | 70 --------- gtsam/base/tests/testLieScalar.cpp | 64 --------- gtsam/base/tests/testLieVector.cpp | 66 --------- gtsam/base/tests/testTestableAssertions.cpp | 35 ----- 12 files changed, 1 insertion(+), 678 deletions(-) delete mode 100644 gtsam/base/LieMatrix.h delete mode 100644 gtsam/base/LieScalar.h delete mode 100644 gtsam/base/LieVector.h delete mode 100644 gtsam/base/deprecated/LieMatrix.h delete mode 100644 gtsam/base/deprecated/LieScalar.h delete mode 100644 gtsam/base/deprecated/LieVector.h delete mode 100644 gtsam/base/tests/testLieMatrix.cpp delete mode 100644 gtsam/base/tests/testLieScalar.cpp delete mode 100644 gtsam/base/tests/testLieVector.cpp delete mode 100644 gtsam/base/tests/testTestableAssertions.cpp diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index cae1d499c6..faeebc97f3 100644 --- a/Using-GTSAM-EXPORT.md +++ b/Using-GTSAM-EXPORT.md @@ -29,7 +29,7 @@ Rule #1 doesn't seem very bad, until you combine it with rule #2 ***Compiler Rule #2*** Anything declared in a header file is not included in a DLL. -When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. LieMatrix) cannot use `GTSAM_EXPORT` in its definition. If LieMatrix is defined with `GTSAM_EXPORT`, then the compiler _must_ find LieMatrix in a DLL. Because LieMatrix is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class. +When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition. If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL. Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class. Also note that when a class that you want to export inherits from another class that is not exportable, this can cause significant issues. According to this [MSVC Warning page](https://docs.microsoft.com/en-us/cpp/error-messages/compiler-warnings/compiler-warning-level-2-c4275?view=vs-2019), it may not strictly be a rule, but we have seen several linker errors when a class that is defined with `GTSAM_EXPORT` extended an Eigen class. In general, it appears that any inheritance of non-exportable class by an exportable class is a bad idea. diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index 99984e7b38..66d3ec7218 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -5,8 +5,5 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base) file(GLOB base_headers_tree "treeTraversal/*.h") install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal) -file(GLOB deprecated_headers "deprecated/*.h") -install(FILES ${deprecated_headers} DESTINATION include/gtsam/base/deprecated) - # Build tests add_subdirectory(tests) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h deleted file mode 100644 index 210bdcc735..0000000000 --- a/gtsam/base/LieMatrix.h +++ /dev/null @@ -1,26 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LieMatrix.h - * @brief External deprecation warning, see deprecated/LieMatrix.h for details - * @author Paul Drews - */ - -#pragma once - -#ifdef _MSC_VER -#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") -#else -#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." -#endif - -#include "gtsam/base/deprecated/LieMatrix.h" diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h deleted file mode 100644 index e159ffa873..0000000000 --- a/gtsam/base/LieScalar.h +++ /dev/null @@ -1,26 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LieScalar.h - * @brief External deprecation warning, see deprecated/LieScalar.h for details - * @author Kai Ni - */ - -#pragma once - -#ifdef _MSC_VER -#pragma message("LieScalar.h is deprecated. Please use double/float instead.") -#else - #warning "LieScalar.h is deprecated. Please use double/float instead." -#endif - -#include diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h deleted file mode 100644 index a7491d8042..0000000000 --- a/gtsam/base/LieVector.h +++ /dev/null @@ -1,26 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LieVector.h - * @brief Deprecation warning for LieVector, see deprecated/LieVector.h for details. - * @author Paul Drews - */ - -#pragma once - -#ifdef _MSC_VER -#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.") -#else -#warning "LieVector.h is deprecated. Please use Eigen::Vector instead." -#endif - -#include diff --git a/gtsam/base/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h deleted file mode 100644 index a3d0a43289..0000000000 --- a/gtsam/base/deprecated/LieMatrix.h +++ /dev/null @@ -1,152 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LieMatrix.h - * @brief A wrapper around Matrix providing Lie compatibility - * @author Richard Roberts and Alex Cunningham - */ - -#pragma once - -#include - -#include -#include - -namespace gtsam { - -/** - * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieMatrix : public Matrix { - - /// @name Constructors - /// @{ - enum { dimension = Eigen::Dynamic }; - - /** default constructor - only for serialize */ - LieMatrix() {} - - /** initialize from a normal matrix */ - LieMatrix(const Matrix& v) : Matrix(v) {} - - template - LieMatrix(const M& v) : Matrix(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} -#endif - - /** constructor with size and initial data, row order ! */ - LieMatrix(size_t m, size_t n, const double* const data) : - Matrix(Eigen::Map(data, m, n)) {} - - /// @} - /// @name Testable interface - /// @{ - - /** print @param s optional string naming the object */ - void print(const std::string& name = "") const { - gtsam::print(matrix(), name); - } - /** equality up to tolerance */ - inline bool equals(const LieMatrix& expected, double tol=1e-5) const { - return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** get the underlying matrix */ - inline Matrix matrix() const { - return static_cast(*this); - } - - /// @} - - /// @name Group - /// @{ - LieMatrix compose(const LieMatrix& q) { return (*this)+q;} - LieMatrix between(const LieMatrix& q) { return q-(*this);} - LieMatrix inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} - LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieMatrix& p) {return p.vector();} - static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return size(); } - - /** Convert to vector, is done row-wise - TODO why? */ - inline Vector vector() const { - Vector result(size()); - typedef Eigen::Matrix RowMajor; - Eigen::Map(&result(0), rows(), cols()) = *this; - return result; - } - - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieMatrix identity() { - throw std::runtime_error("LieMatrix::identity(): Don't use this function"); - return LieMatrix(); - } - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("Matrix", - boost::serialization::base_object(*this)); - - } - -}; - - -template<> -struct traits : public internal::VectorSpace { - - // Override Retract, as the default version does not know how to initialize - static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { - if (H1) *H1 = Eye(origin); - if (H2) *H2 = Eye(origin); - typedef const Eigen::Matrix RowMajor; - return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); - } - -}; - -} // \namespace gtsam diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h deleted file mode 100644 index 6c9a5f766d..0000000000 --- a/gtsam/base/deprecated/LieScalar.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LieScalar.h - * @brief A wrapper around scalar providing Lie compatibility - * @author Kai Ni - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /** - * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ - struct LieScalar { - - enum { dimension = 1 }; - - /** default constructor */ - LieScalar() : d_(0.0) {} - - /** wrap a double */ - /*explicit*/ LieScalar(double d) : d_(d) {} - - /** access the underlying value */ - double value() const { return d_; } - - /** Automatic conversion to underlying value */ - operator double() const { return d_; } - - /** convert vector */ - Vector1 vector() const { Vector1 v; v< - struct traits : public internal::ScalarTraits {}; - -} // \namespace gtsam diff --git a/gtsam/base/deprecated/LieVector.h b/gtsam/base/deprecated/LieVector.h deleted file mode 100644 index 745189c3de..0000000000 --- a/gtsam/base/deprecated/LieVector.h +++ /dev/null @@ -1,121 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LieVector.h - * @brief A wrapper around vector providing Lie compatibility - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/** - * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieVector : public Vector { - - enum { dimension = Eigen::Dynamic }; - - /** default constructor - should be unnecessary */ - LieVector() {} - - /** initialize from a normal vector */ - LieVector(const Vector& v) : Vector(v) {} - - template - LieVector(const V& v) : Vector(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieVector(const Eigen::Matrix& v) : Vector(v) {} -#endif - - /** wrap a double */ - LieVector(double d) : Vector((Vector(1) << d).finished()) {} - - /** constructor with size and initial data, row order ! */ - LieVector(size_t m, const double* const data) : Vector(m) { - for (size_t i = 0; i < m; i++) (*this)(i) = data[i]; - } - - /// @name Testable - /// @{ - void print(const std::string& name="") const { - gtsam::print(vector(), name); - } - bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } - /// @} - - /// @name Group - /// @{ - LieVector compose(const LieVector& q) { return (*this)+q;} - LieVector between(const LieVector& q) { return q-(*this);} - LieVector inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieVector& q) { return between(q).vector();} - LieVector retract(const Vector& v) {return compose(LieVector(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieVector& p) {return p.vector();} - static LieVector Expmap(const Vector& v) { return LieVector(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** get the underlying vector */ - Vector vector() const { - return static_cast(*this); - } - - /** Returns dimensionality of the tangent space */ - size_t dim() const { return this->size(); } - - /** identity - NOTE: no known size at compile time - so zero length */ - static LieVector identity() { - throw std::runtime_error("LieVector::identity(): Don't use this function"); - return LieVector(); - } - - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("Vector", - boost::serialization::base_object(*this)); - } -}; - - -template<> -struct traits : public internal::VectorSpace {}; - -} // \namespace gtsam diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp deleted file mode 100644 index 8c68bf8a00..0000000000 --- a/gtsam/base/tests/testLieMatrix.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testLieMatrix.cpp - * @author Richard Roberts - */ - -#include -#include -#include -#include - -using namespace gtsam; - -GTSAM_CONCEPT_TESTABLE_INST(LieMatrix) -GTSAM_CONCEPT_LIE_INST(LieMatrix) - -/* ************************************************************************* */ -TEST( LieMatrix, construction ) { - Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); - LieMatrix lie1(m), lie2(m); - - EXPECT(traits::GetDimension(m) == 4); - EXPECT(assert_equal(m, lie1.matrix())); - EXPECT(assert_equal(lie1, lie2)); -} - -/* ************************************************************************* */ -TEST( LieMatrix, other_constructors ) { - Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0).finished(); - LieMatrix exp(init); - double data[] = {10,30,20,40}; - LieMatrix b(2,2,data); - EXPECT(assert_equal(exp, b)); -} - -/* ************************************************************************* */ -TEST(LieMatrix, retract) { - LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0).finished()); - Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); - - LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); - LieMatrix actual = traits::Retract(init,update); - - EXPECT(assert_equal(expected, actual)); - - Vector expectedUpdate = update; - Vector actualUpdate = traits::Local(init,actual); - - EXPECT(assert_equal(expectedUpdate, actualUpdate)); - - Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); - Vector actualLogmap = traits::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); - EXPECT(assert_equal(expectedLogmap, actualLogmap)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - - diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp deleted file mode 100644 index 74f5e0d414..0000000000 --- a/gtsam/base/tests/testLieScalar.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testLieScalar.cpp - * @author Kai Ni - */ - -#include -#include -#include -#include - -using namespace gtsam; - -GTSAM_CONCEPT_TESTABLE_INST(LieScalar) -GTSAM_CONCEPT_LIE_INST(LieScalar) - -const double tol=1e-9; - -//****************************************************************************** -TEST(LieScalar , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(LieScalar , Invariants) { - LieScalar lie1(2), lie2(3); - CHECK(check_group_invariants(lie1, lie2)); - CHECK(check_manifold_invariants(lie1, lie2)); -} - -/* ************************************************************************* */ -TEST( testLieScalar, construction ) { - double d = 2.; - LieScalar lie1(d), lie2(d); - - EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); - EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); - EXPECT(traits::dimension == 1); - EXPECT(assert_equal(lie1, lie2)); -} - -/* ************************************************************************* */ -TEST( testLieScalar, localCoordinates ) { - LieScalar lie1(1.), lie2(3.); - - Vector1 actual = traits::Local(lie1, lie2); - EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp deleted file mode 100644 index 76c4fc490a..0000000000 --- a/gtsam/base/tests/testLieVector.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testLieVector.cpp - * @author Alex Cunningham - */ - -#include -#include -#include -#include - -using namespace gtsam; - -GTSAM_CONCEPT_TESTABLE_INST(LieVector) -GTSAM_CONCEPT_LIE_INST(LieVector) - -//****************************************************************************** -TEST(LieVector , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(LieVector , Invariants) { - Vector v = Vector3(1.0, 2.0, 3.0); - LieVector lie1(v), lie2(v); - check_manifold_invariants(lie1, lie2); -} - -//****************************************************************************** -TEST( testLieVector, construction ) { - Vector v = Vector3(1.0, 2.0, 3.0); - LieVector lie1(v), lie2(v); - - EXPECT(lie1.dim() == 3); - EXPECT(assert_equal(v, lie1.vector())); - EXPECT(assert_equal(lie1, lie2)); -} - -//****************************************************************************** -TEST( testLieVector, other_constructors ) { - Vector init = Vector2(10.0, 20.0); - LieVector exp(init); - double data[] = { 10, 20 }; - LieVector b(2, data); - EXPECT(assert_equal(exp, b)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp deleted file mode 100644 index 305aa7ca93..0000000000 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testTestableAssertions - * @author Alex Cunningham - */ - -#include -#include -#include - -using namespace gtsam; - -/* ************************************************************************* */ -TEST( testTestableAssertions, optional ) { - typedef boost::optional OptionalScalar; - LieScalar x(1.0); - OptionalScalar ox(x), dummy = boost::none; - EXPECT(assert_equal(ox, ox)); - EXPECT(assert_equal(x, ox)); - EXPECT(assert_equal(dummy, dummy)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ From 8dbe5ee1790af2be6ceef6e34d41706b5f607bfc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:46:39 -0500 Subject: [PATCH 07/62] "fixed" Unstable MATLAB examples - untested --- matlab/+gtsam/Contents.m | 12 ------------ .../unstable_examples/+imuSimulator/IMUComparison.m | 8 ++++---- .../+imuSimulator/IMUComparison_with_cov.m | 8 ++++---- .../+imuSimulator/coriolisExample.m | 6 +++--- .../covarianceAnalysisCreateFactorGraph.m | 2 +- .../covarianceAnalysisCreateTrajectory.m | 2 +- matlab/unstable_examples/FlightCameraTransformIMU.m | 2 +- matlab/unstable_examples/IMUKittiExampleVO.m | 4 ++-- 8 files changed, 16 insertions(+), 28 deletions(-) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index fb6d3081e8..77536e5c9d 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -49,9 +49,6 @@ % Ordering - class Ordering, see Doxygen page for details % Value - class Value, see Doxygen page for details % Values - class Values, see Doxygen page for details -% LieScalar - class LieScalar, see Doxygen page for details -% LieVector - class LieVector, see Doxygen page for details -% LieMatrix - class LieMatrix, see Doxygen page for details % NonlinearFactor - class NonlinearFactor, see Doxygen page for details % NonlinearFactorGraph - class NonlinearFactorGraph, see Doxygen page for details % @@ -101,9 +98,6 @@ % BearingFactor2D - class BearingFactor2D, see Doxygen page for details % BearingFactor3D - class BearingFactor3D, see Doxygen page for details % BearingRangeFactor2D - class BearingRangeFactor2D, see Doxygen page for details -% BetweenFactorLieMatrix - class BetweenFactorLieMatrix, see Doxygen page for details -% BetweenFactorLieScalar - class BetweenFactorLieScalar, see Doxygen page for details -% BetweenFactorLieVector - class BetweenFactorLieVector, see Doxygen page for details % BetweenFactorPoint2 - class BetweenFactorPoint2, see Doxygen page for details % BetweenFactorPoint3 - class BetweenFactorPoint3, see Doxygen page for details % BetweenFactorPose2 - class BetweenFactorPose2, see Doxygen page for details @@ -116,9 +110,6 @@ % GenericStereoFactor3D - class GenericStereoFactor3D, see Doxygen page for details % NonlinearEqualityCal3_S2 - class NonlinearEqualityCal3_S2, see Doxygen page for details % NonlinearEqualityCalibratedCamera - class NonlinearEqualityCalibratedCamera, see Doxygen page for details -% NonlinearEqualityLieMatrix - class NonlinearEqualityLieMatrix, see Doxygen page for details -% NonlinearEqualityLieScalar - class NonlinearEqualityLieScalar, see Doxygen page for details -% NonlinearEqualityLieVector - class NonlinearEqualityLieVector, see Doxygen page for details % NonlinearEqualityPoint2 - class NonlinearEqualityPoint2, see Doxygen page for details % NonlinearEqualityPoint3 - class NonlinearEqualityPoint3, see Doxygen page for details % NonlinearEqualityPose2 - class NonlinearEqualityPose2, see Doxygen page for details @@ -129,9 +120,6 @@ % NonlinearEqualityStereoPoint2 - class NonlinearEqualityStereoPoint2, see Doxygen page for details % PriorFactorCal3_S2 - class PriorFactorCal3_S2, see Doxygen page for details % PriorFactorCalibratedCamera - class PriorFactorCalibratedCamera, see Doxygen page for details -% PriorFactorLieMatrix - class PriorFactorLieMatrix, see Doxygen page for details -% PriorFactorLieScalar - class PriorFactorLieScalar, see Doxygen page for details -% PriorFactorLieVector - class PriorFactorLieVector, see Doxygen page for details % PriorFactorPoint2 - class PriorFactorPoint2, see Doxygen page for details % PriorFactorPoint3 - class PriorFactorPoint3, see Doxygen page for details % PriorFactorPose2 - class PriorFactorPose2, see Doxygen page for details diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m index 871f023ef7..ccc975d849 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -51,13 +51,13 @@ initialValues = Values; initialValues.insert(symbol('x',0), currentPoseGlobal); -initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('v',0), currentVelocityGlobal); initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); initialFactors = NonlinearFactorGraph; initialFactors.add(PriorFactorPose3(symbol('x',0), ... currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0))); -initialFactors.add(PriorFactorLieVector(symbol('v',0), ... - LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0))); +initialFactors.add(PriorFactorVector(symbol('v',0), ... + currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, 1.0))); initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0))); @@ -96,7 +96,7 @@ initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); else initialPose = Pose3; - initialVel = LieVector(velocity); + initialVel = velocity; end initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m index 450697de05..6adc8e9dc2 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -43,15 +43,15 @@ initialValues = Values; initialValues.insert(symbol('x',0), currentPoseGlobal); -initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('v',0), currentVelocityGlobal); initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); initialFactors = NonlinearFactorGraph; % Prior on initial pose initialFactors.add(PriorFactorPose3(symbol('x',0), ... currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x))); % Prior on initial velocity -initialFactors.add(PriorFactorLieVector(symbol('v',0), ... - LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v))); +initialFactors.add(PriorFactorVector(symbol('v',0), ... + currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, sigma_init_v))); % Prior on initial bias initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b))); @@ -91,7 +91,7 @@ initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); else initialPose = Pose3; - initialVel = LieVector(velocity); + initialVel = velocity; end initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index ee4deb4335..61dc78d968 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -175,9 +175,9 @@ % known initial conditions currentPoseEstimate = currentPoseFixedGT; if navFrameRotating == 1 - currentVelocityEstimate = LieVector(currentVelocityRotatingGT); + currentVelocityEstimate = currentVelocityRotatingGT; else - currentVelocityEstimate = LieVector(currentVelocityFixedGT); + currentVelocityEstimate = currentVelocityFixedGT; end % Set Priors @@ -186,7 +186,7 @@ newValues.insert(currentBiasKey, zeroBias); % Initial values, same for IMU types 1 and 2 newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); % Store data diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 07f146dcb8..037065ac54 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -27,7 +27,7 @@ if options.includeIMUFactors == 1 currentVelKey = symbol('v', 0); currentVel = values.atPoint3(currentVelKey); - graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); + graph.add(PriorFactorVector(currentVelKey, currentVel, noiseModels.noiseVel)); currentBiasKey = symbol('b', 0); currentBias = values.atPoint3(currentBiasKey); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 3d8a9b5d28..5fb6589d6e 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -82,7 +82,7 @@ end % Add Values: velocity and bias - values.insert(currentVelKey, LieVector(currentVel)); + values.insert(currentVelKey, currentVel); values.insert(currentBiasKey, metadata.imu.zeroBias); end diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index d2f2bc34d7..aeac2e2431 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -167,7 +167,7 @@ %% priors on first two poses if i < 3 - % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + % fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); end diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m index 6434e750ae..f35d365127 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -46,7 +46,7 @@ %% Get initial conditions for the estimated trajectory currentPoseGlobal = Pose3; -currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); @@ -88,7 +88,7 @@ newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentBiasKey, currentBias); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); else t_previous = timestamps(measurementIndex-1, 1); From 9518346161c21715d407bac5ff553f054b6fc876 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:59:28 -0500 Subject: [PATCH 08/62] Fix unstable c++ examples --- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 10 ++--- gtsam_unstable/slam/serialization.cpp | 16 -------- .../tests/testGaussMarkov1stOrderFactor.cpp | 41 ++++++++++--------- .../slam/tests/testSerialization.cpp | 2 +- 4 files changed, 27 insertions(+), 42 deletions(-) diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 0e2aebd7fe..b053b13f82 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -372,15 +372,15 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); Matrix H_pos_angles = Z_3x3; - Matrix H_vel_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_vel_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_angles_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 88a94fd512..bed11e5356 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -5,8 +5,6 @@ * @author Alex Cunningham */ -#include -#include #include #include @@ -31,8 +29,6 @@ using namespace gtsam; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; typedef PriorFactor PriorFactorPoint2; typedef PriorFactor PriorFactorStereoPoint2; typedef PriorFactor PriorFactorPoint3; @@ -46,8 +42,6 @@ typedef PriorFactor PriorFactorCalibratedCamera; typedef PriorFactor PriorFactorPinholeCameraCal3_S2; typedef PriorFactor PriorFactorStereoCamera; -typedef BetweenFactor BetweenFactorLieVector; -typedef BetweenFactor BetweenFactorLieMatrix; typedef BetweenFactor BetweenFactorPoint2; typedef BetweenFactor BetweenFactorPoint3; typedef BetweenFactor BetweenFactorRot2; @@ -55,8 +49,6 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; typedef NonlinearEquality NonlinearEqualityPoint2; typedef NonlinearEquality NonlinearEqualityStereoPoint2; typedef NonlinearEquality NonlinearEqualityPoint3; @@ -112,8 +104,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -GTSAM_VALUE_EXPORT(gtsam::LieVector); -GTSAM_VALUE_EXPORT(gtsam::LieMatrix); GTSAM_VALUE_EXPORT(gtsam::Point2); GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); GTSAM_VALUE_EXPORT(gtsam::Point3); @@ -133,8 +123,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); @@ -147,8 +135,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); @@ -156,8 +142,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 8692cf5841..ed4092c600 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -16,22 +16,23 @@ * @date Jan 17, 2012 */ -#include -#include -#include -#include #include -#include +#include +#include +#include +#include +#include using namespace std::placeholders; using namespace std; using namespace gtsam; //! Factors -typedef GaussMarkov1stOrderFactor GaussMarkovFactor; +typedef GaussMarkov1stOrderFactor GaussMarkovFactor; /* ************************************************************************* */ -LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) { +Vector predictionError(const Vector& v1, const Vector& v2, + const GaussMarkovFactor factor) { return factor.evaluateError(v1, v2); } @@ -58,29 +59,29 @@ TEST( GaussMarkovFactor, error ) Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = Vector3(100.0, 150.0, 10.0); + Vector3 tau(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); - LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0)); - LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0)); + Vector3 v1(10.0, 12.0, 13.0); + Vector3 v2(10.0, 15.0, 14.0); // Create two nodes linPoint.insert(x1, v1); linPoint.insert(x2, v2); GaussMarkovFactor factor(x1, x2, delta_t, tau, model); - Vector Err1( factor.evaluateError(v1, v2) ); + Vector3 error1 = factor.evaluateError(v1, v2); // Manually calculate the error - Vector alpha(tau.size()); - Vector alpha_v1(tau.size()); + Vector3 alpha(tau.size()); + Vector3 alpha_v1(tau.size()); for(int i=0; i #include -#include +#include #include #include From a38de285357d5628a75a071fdff293b00079e9a2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 15:00:49 -0500 Subject: [PATCH 09/62] Tested python wrapper without Lie* --- gtsam/basis/ParameterMatrix.h | 2 +- python/CMakeLists.txt | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index df2d9f62e9..eddcbfeaec 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -153,7 +153,7 @@ class ParameterMatrix { return matrix_ * other; } - /// @name Vector Space requirements, following LieMatrix + /// @name Vector Space requirements /// @{ /** diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index d3b20e3126..b39e067b07 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -33,8 +33,6 @@ add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_uns set(ignore gtsam::Point2 gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet @@ -116,8 +114,6 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set(ignore gtsam::Point2 gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet From 6d0c55901cbe9aeb02f131d73bb3167da8b4e500 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 15:49:47 -0500 Subject: [PATCH 10/62] Global replace to V42 --- .github/scripts/python.sh | 2 +- .github/scripts/unix.sh | 2 +- .github/workflows/build-special.yml | 2 +- README.md | 4 ++-- cmake/HandleGeneralOptions.cmake | 2 +- cmake/HandlePrintConfiguration.cmake | 2 +- .../Eigen/Eigen/src/Core/CwiseNullaryOp.h | 4 ++-- .../3rdparty/Eigen/Eigen/src/Core/DenseBase.h | 4 ++-- .../3rdparty/Eigen/Eigen/src/Core/EigenBase.h | 2 +- .../Eigen/Eigen/src/Core/TriangularMatrix.h | 6 ++--- .../Eigen/Eigen/src/Core/util/Constants.h | 4 ++-- .../Eigen/Eigen/src/Geometry/AlignedBox.h | 4 ++-- .../Eigen/src/Geometry/ParametrizedLine.h | 2 +- .../Eigen/Eigen/src/Geometry/Scaling.h | 8 +++---- .../src/SparseCholesky/SimplicialCholesky.h | 2 +- .../Eigen/src/SparseCore/MappedSparseMatrix.h | 2 +- .../src/SparseExtra/DynamicSparseMatrix.h | 10 ++++---- .../include/GeographicLib/NormalGravity.hpp | 2 +- .../include/GeographicLib/Utility.hpp | 2 +- gtsam/base/TestableAssertions.h | 4 +++- gtsam/base/Vector.h | 6 +++-- gtsam/config.h.in | 2 +- gtsam/geometry/Cal3.h | 2 +- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/SimpleCamera.cpp | 2 +- gtsam/geometry/SimpleCamera.h | 2 +- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/inference/EliminateableFactorGraph.h | 14 +++++------ gtsam/inference/Factor.h | 1 - gtsam/linear/GaussianConditional.h | 7 +++--- gtsam/linear/GaussianFactorGraph.h | 10 ++++---- gtsam/linear/NoiseModel.h | 2 ++ gtsam/navigation/AHRSFactor.h | 23 +++++++++++-------- gtsam/navigation/ImuBias.h | 18 +++++++-------- gtsam/nonlinear/ExpressionFactor.h | 4 ++-- gtsam/nonlinear/ExtendedKalmanFilter.h | 2 ++ gtsam/nonlinear/Marginals.h | 8 +++---- gtsam/nonlinear/NonlinearFactorGraph.h | 10 ++++---- gtsam/slam/dataset.cpp | 10 ++++---- gtsam/slam/dataset.h | 22 +++++++++--------- gtsam_unstable/slam/serialization.cpp | 2 +- 41 files changed, 116 insertions(+), 105 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 3f5701281c..6cc62d2b06 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -75,7 +75,7 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \ -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \ -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 9689d346ca..d890577b65 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -64,7 +64,7 @@ function configure() -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=${GTSAM_ALLOW_DEPRECATED_SINCE_V42:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 647b9c0f18..d357b9a340 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -110,7 +110,7 @@ jobs: - name: Set Allow Deprecated Flag if: matrix.flag == 'deprecated' run: | - echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV + echo "GTSAM_ALLOW_DEPRECATED_SINCE_V42=ON" >> $GITHUB_ENV echo "Allow deprecated since version 4.1" - name: Set Use Quaternions Flag diff --git a/README.md b/README.md index ee5746e1cf..52ac0a5d88 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ As of Dec 2021, the `develop` branch is officially in "Pre 4.2" mode. A great new feature we will be adding in 4.2 is *hybrid inference* a la DCSLAM (Kevin Doherty et al) and we envision several API-breaking changes will happen in the discrete folder. -In addition, features deprecated in 4.1 will be removed. Please use the last [4.1.1 release](https://github.com/borglab/gtsam/releases/tag/4.1.1) if you need those features. However, most (not all, unfortunately) are easily converted and can be tracked down (in 4.1.1) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41`. +In addition, features deprecated in 4.1 will be removed. Please use the last [4.1.1 release](https://github.com/borglab/gtsam/releases/tag/4.1.1) if you need those features. However, most (not all, unfortunately) are easily converted and can be tracked down (in 4.1.1) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42`. ## What is GTSAM? @@ -57,7 +57,7 @@ GTSAM 4 introduces several new features, most notably Expressions and a Python t GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods. -GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. +GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. ## Wrappers diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 64c239f393..7c8f8533f3 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -25,7 +25,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will a option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V42 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index ad6ac5c5cf..43ee5b57b4 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -86,7 +86,7 @@ print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as defaul print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1") +print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V42} "Allow features deprecated in GTSAM 4.1") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h index ddd607e383..4ffd0057b1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h @@ -215,7 +215,7 @@ DenseBase::Constant(const Scalar& value) return DenseBase::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_constant_op(value)); } -/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) +/** @deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) * * \sa LinSpaced(Index,Scalar,Scalar), setLinSpaced(Index,const Scalar&,const Scalar&) */ @@ -227,7 +227,7 @@ DenseBase::LinSpaced(Sequential_t, Index size, const Scalar& low, const return DenseBase::NullaryExpr(size, internal::linspaced_op(low,high,size)); } -/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) +/** @deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) * * \sa LinSpaced(Scalar,Scalar) */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index 90066ae73f..8b2733814e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -298,7 +298,7 @@ template class DenseBase /** \internal * Copies \a other into *this without evaluating other. \returns a reference to *this. - * \deprecated */ + * @deprecated */ template EIGEN_DEVICE_FUNC Derived& lazyAssign(const DenseBase& other); @@ -306,7 +306,7 @@ template class DenseBase EIGEN_DEVICE_FUNC CommaInitializer operator<< (const Scalar& s); - /** \deprecated it now returns \c *this */ + /** @deprecated it now returns \c *this */ template EIGEN_DEPRECATED const Derived& flagged() const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h index b195506a91..bf30e03ffd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h @@ -32,7 +32,7 @@ template struct EigenBase /** \brief The interface type of indices * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE. - * \deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. + * @deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. * \sa StorageIndex, \ref TopicPreprocessorDirectives. */ typedef Eigen::Index Index; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index 667ef09dc1..9c76906fec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -435,12 +435,12 @@ template class TriangularViewImpl<_Mat TriangularViewType& operator=(const TriangularViewImpl& other) { return *this = other.derived().nestedExpression(); } - /** \deprecated */ + /** @deprecated */ template EIGEN_DEVICE_FUNC void lazyAssign(const TriangularBase& other); - /** \deprecated */ + /** @deprecated */ template EIGEN_DEVICE_FUNC void lazyAssign(const MatrixBase& other); @@ -523,7 +523,7 @@ template class TriangularViewImpl<_Mat call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op()); } - /** \deprecated + /** @deprecated * Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */ template EIGEN_DEVICE_FUNC diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h index 7587d68424..9602332c8a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h @@ -65,7 +65,7 @@ const unsigned int RowMajorBit = 0x1; const unsigned int EvalBeforeNestingBit = 0x2; /** \ingroup flags - * \deprecated + * @deprecated * means the expression should be evaluated before any assignment */ EIGEN_DEPRECATED const unsigned int EvalBeforeAssigningBit = 0x4; // FIXME deprecated @@ -149,7 +149,7 @@ const unsigned int LvalueBit = 0x20; */ const unsigned int DirectAccessBit = 0x40; -/** \deprecated \ingroup flags +/** @deprecated \ingroup flags * * means the first coefficient packet is guaranteed to be aligned. * An expression cannot has the AlignedBit without the PacketAccessBit flag. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h index 066eae4f92..96c100245e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h @@ -84,10 +84,10 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns the dimension in which the box holds */ EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } - /** \deprecated use isEmpty() */ + /** @deprecated use isEmpty() */ EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); } - /** \deprecated use setEmpty() */ + /** @deprecated use setEmpty() */ EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); } /** \returns true if the box is empty. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h index 1e985d8cde..60ca9b7f37 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h @@ -170,7 +170,7 @@ EIGEN_DEVICE_FUNC inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options> } -/** \deprecated use intersectionParameter() +/** @deprecated use intersectionParameter() * \returns the parameter value of the intersection between \c *this and the given \a hyperplane */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h index f58ca03d94..81a5bcafce 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h @@ -142,13 +142,13 @@ template inline const DiagonalWrapper Scaling(const MatrixBase& coeffs) { return coeffs.asDiagonal(); } -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling2f; -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling2d; -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling3f; -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling3d; //@} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h index 2907f65296..f2693fc81b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -493,7 +493,7 @@ template } }; -/** \deprecated use SimplicialLDLT or class SimplicialLLT +/** @deprecated use SimplicialLDLT or class SimplicialLLT * \ingroup SparseCholesky_Module * \class SimplicialCholesky * diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h index 67718c85be..25ca27fe59 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h @@ -12,7 +12,7 @@ namespace Eigen { -/** \deprecated Use Map > +/** @deprecated Use Map > * \class MappedSparseMatrix * * \brief Sparse matrix diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h index 0ffbc43d23..1f0379c152 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h @@ -12,7 +12,7 @@ namespace Eigen { -/** \deprecated use a SparseMatrix in an uncompressed mode +/** @deprecated use a SparseMatrix in an uncompressed mode * * \class DynamicSparseMatrix * @@ -291,7 +291,7 @@ template public: - /** \deprecated + /** @deprecated * Set the matrix to zero and reserve the memory for \a reserveSize nonzero coefficients. */ EIGEN_DEPRECATED void startFill(Index reserveSize = 1000) { @@ -299,7 +299,7 @@ template reserve(reserveSize); } - /** \deprecated use insert() + /** @deprecated use insert() * inserts a nonzero coefficient at given coordinates \a row, \a col and returns its reference assuming that: * 1 - the coefficient does not exist yet * 2 - this the coefficient with greater inner coordinate for the given outer coordinate. @@ -315,7 +315,7 @@ template return insertBack(outer,inner); } - /** \deprecated use insert() + /** @deprecated use insert() * Like fill() but with random inner coordinates. * Compared to the generic coeffRef(), the unique limitation is that we assume * the coefficient does not exist yet. @@ -325,7 +325,7 @@ template return insert(row,col); } - /** \deprecated use finalize() + /** @deprecated use finalize() * Does nothing. Provided for compatibility with SparseMatrix. */ EIGEN_DEPRECATED void endFill() {} diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp index 2c4955f745..13a21c657c 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp @@ -142,7 +142,7 @@ namespace GeographicLib { NormalGravity(real a, real GM, real omega, real f_J2, bool geometricp = true); /** - * \deprecated Old constructor for the normal gravity. + * @deprecated Old constructor for the normal gravity. * * @param[in] a equatorial radius (meters). * @param[in] GM mass constant of the ellipsoid diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp index 9990e47e91..a9b0129d47 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp @@ -380,7 +380,7 @@ namespace GeographicLib { return x; } /** - * \deprecated An old name for val(s). + * @deprecated An old name for val(s). **********************************************************************/ template // GEOGRAPHICLIB_DEPRECATED("Use new Utility::val(s)") diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index c86fbb6d22..e5bd34d19c 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -80,9 +80,10 @@ bool assert_equal(const V& expected, const boost::optional& actual, do return assert_equal(expected, *actual, tol); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * Version of assert_equals to work with vectors - * \deprecated: use container equals instead + * @deprecated: use container equals instead */ template bool GTSAM_DEPRECATED assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { @@ -108,6 +109,7 @@ bool GTSAM_DEPRECATED assert_equal(const std::vector& expected, const std::ve } return true; } +#endif /** * Function for comparing maps of testable->testable diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index a057da46b4..36dc2288da 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -203,15 +203,16 @@ inline double inner_prod(const V1 &a, const V2& b) { return a.dot(b); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * BLAS Level 1 scal: x <- alpha*x - * \deprecated: use operators instead + * @deprecated: use operators instead */ inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; } /** * BLAS Level 1 axpy: y <- alpha*x + y - * \deprecated: use operators instead + * @deprecated: use operators instead */ template inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) { @@ -222,6 +223,7 @@ inline void axpy(double alpha, const Vector& x, SubVector y) { assert (y.size()==x.size()); y += alpha * x; } +#endif /** * house(x,j) computes HouseHolder vector v and scaling factor beta diff --git a/gtsam/config.h.in b/gtsam/config.h.in index e7623c52b7..d47329a627 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -70,7 +70,7 @@ #cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION // Make sure dependent projects that want it can see deprecated functions -#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V42 // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 08ce4c1e67..128c217f93 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -170,7 +170,7 @@ class GTSAM_EXPORT Cal3 { return K; } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** @deprecated The following function has been deprecated, use K above */ Matrix3 matrix() const { return K(); } #endif diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 0d7c1be9d5..2285b2dbbe 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -97,7 +97,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { Vector3 vector() const; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// get parameter u0 inline double u0() const { return u0_; } diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index d1a5ed3308..3b871b4686 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,7 +21,7 @@ namespace gtsam { -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 5ff6b9816f..f0776c2e2a 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -37,7 +37,7 @@ namespace gtsam { using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x * Use PinholeCameraCal3_S2 instead diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 18a25c5534..173ccf05b8 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 static const Cal3_S2 K(625, 625, 0, 0, 0); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 579eed7f9d..c904d2f7ff 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -288,8 +288,8 @@ namespace gtsam { FactorGraphType& asDerived() { return static_cast(*this); } public: - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated ordering and orderingType shouldn't both be specified */ + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated ordering and orderingType shouldn't both be specified */ boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Ordering& ordering, const Eliminate& function, @@ -298,7 +298,7 @@ namespace gtsam { return eliminateSequential(ordering, function, variableIndex); } - /** \deprecated orderingType specified first for consistency */ + /** @deprecated orderingType specified first for consistency */ boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, @@ -306,7 +306,7 @@ namespace gtsam { return eliminateSequential(orderingType, function, variableIndex); } - /** \deprecated ordering and orderingType shouldn't both be specified */ + /** @deprecated ordering and orderingType shouldn't both be specified */ boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, @@ -315,7 +315,7 @@ namespace gtsam { return eliminateMultifrontal(ordering, function, variableIndex); } - /** \deprecated orderingType specified first for consistency */ + /** @deprecated orderingType specified first for consistency */ boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, @@ -323,7 +323,7 @@ namespace gtsam { return eliminateMultifrontal(orderingType, function, variableIndex); } - /** \deprecated */ + /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesNet( boost::variant variables, boost::none_t, @@ -332,7 +332,7 @@ namespace gtsam { return marginalMultifrontalBayesNet(variables, function, variableIndex); } - /** \deprecated */ + /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesTree( boost::variant variables, boost::none_t, diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 1ca97cea51..e6a8dcc604 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -153,7 +153,6 @@ typedef FastSet FactorIndexSet; const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; - protected: /// check equality bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 0ea597f99a..d93f65b425 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -125,12 +125,11 @@ namespace gtsam { /** Performs transpose backsubstition in place on values */ void solveTransposeInPlace(VectorValues& gy) const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ - void scaleFrontalsBySigma(VectorValues& gy) const; - - // FIXME: deprecated flag doesn't appear to exist? - // __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const; + void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; +#endif private: /** Serialization function */ diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5ccb0ce91b..7bee4c9fb5 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -396,11 +396,11 @@ namespace gtsam { public: -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated */ - VectorValues optimize(boost::none_t, - const Eliminate& function = - EliminationTraitsType::DefaultEliminate) const { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated */ + VectorValues GTSAM_DEPRECATED + optimize(boost::none_t, const Eliminate& function = + EliminationTraitsType::DefaultEliminate) const { return optimize(function); } #endif diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 54b9d955fb..ba9c97efbd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -730,6 +730,7 @@ namespace gtsam { } // namespace noiseModel +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** Note, deliberately not in noiseModel namespace. * Deprecated. Only for compatibility with previous version. */ @@ -738,6 +739,7 @@ namespace gtsam { typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; +#endif /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 1ab2d7cdc6..94a9665542 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -104,14 +104,15 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated constructor - PreintegratedAhrsMeasurements(const Vector3& biasHat, - const Matrix3& measuredOmegaCovariance) - : PreintegratedRotation(boost::make_shared()), - biasHat_(biasHat) { + GTSAM_DEPRECATED PreintegratedAhrsMeasurements( + const Vector3& biasHat, const Matrix3& measuredOmegaCovariance) + : PreintegratedRotation(boost::make_shared()), biasHat_(biasHat) { p_->gyroscopeCovariance = measuredOmegaCovariance; resetIntegration(); } +#endif private: @@ -186,20 +187,24 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { const Rot3& rot_i, const Vector3& bias, const PreintegratedAhrsMeasurements preintegratedMeasurements); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated name typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; /// @deprecated constructor - AHRSFactor(Key rot_i, Key rot_j, Key bias, + GTSAM_DEPRECATED AHRSFactor( + Key rot_i, Key rot_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor = boost::none); /// @deprecated static function - static Rot3 predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor = boost::none); + static Rot3 GTSAM_DEPRECATED + predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); +#endif private: diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index fad952232f..7557c47324 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -131,30 +131,30 @@ class GTSAM_EXPORT ConstantBias { /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @name Deprecated /// @{ - ConstantBias inverse() { - return -(*this); - } - ConstantBias compose(const ConstantBias& q) { + ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); } + ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) { return (*this) + q; } - ConstantBias between(const ConstantBias& q) { + ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) { return q - (*this); } - Vector6 localCoordinates(const ConstantBias& q) { + Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) { return between(q).vector(); } - ConstantBias retract(const Vector6& v) { + ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) { return compose(ConstantBias(v)); } - static Vector6 Logmap(const ConstantBias& p) { + static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) { return p.vector(); } - static ConstantBias Expmap(const Vector6& v) { + static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) { return ConstantBias(v); } /// @} +#endif private: diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index d22690cf39..11bf873e7f 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -295,14 +295,14 @@ struct traits> // ExpressionFactorN -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V41) +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42) /** * Binary specialization of ExpressionFactor meant as a base class for binary * factors. Enforces an 'expression' method with two keys, and provides * 'evaluateError'. Derived class (a binary factor!) needs to call 'initialize'. * * \sa ExpressionFactorN - * \deprecated Prefer the more general ExpressionFactorN<>. + * @deprecated Prefer the more general ExpressionFactorN<>. */ template class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 77bb1ca6ca..df27d16ff0 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -51,9 +51,11 @@ class ExtendedKalmanFilter { typedef boost::shared_ptr > shared_ptr; typedef VALUE T; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 //@deprecated: any NoiseModelFactor will do, as long as they have the right keys typedef NoiseModelFactor2 MotionFactor; typedef NoiseModelFactor1 MeasurementFactor; +#endif protected: T x_; // linearization point diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 947274a97e..028545d019 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -131,16 +131,16 @@ class GTSAM_EXPORT Marginals { void computeBayesTree(const Ordering& ordering); public: -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated argument order changed due to removing boost::optional */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated argument order changed due to removing boost::optional */ GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} - /** \deprecated argument order changed due to removing boost::optional */ + /** @deprecated argument order changed due to removing boost::optional */ GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} - /** \deprecated argument order changed due to removing boost::optional */ + /** @deprecated argument order changed due to removing boost::optional */ GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} #endif diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 84b1516e95..160e469241 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -250,25 +250,25 @@ namespace gtsam { public: -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return linearizeToHessianFactor(values, dampen);} - /** \deprecated */ + /** @deprecated */ Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} - /** \deprecated */ + /** @deprecated */ void GTSAM_DEPRECATED saveGraph( std::ostream& os, const Values& values = Values(), const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { dot(os, values, keyFormatter, graphvizFormatting); } - /** \deprecated */ + /** @deprecated */ void GTSAM_DEPRECATED saveGraph(const std::string& filename, const Values& values, const GraphvizFormatting& graphvizFormatting, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d7e925bd95..0684063de2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1304,14 +1304,14 @@ parse3DFactors(const std::string &filename, return parseFactors(filename, model, maxIndex); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -std::map parse3DPoses(const std::string &filename, - size_t maxIndex) { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +std::map GTSAM_DEPRECATED +parse3DPoses(const std::string &filename, size_t maxIndex) { return parseVariables(filename, maxIndex); } -std::map parse3DLandmarks(const std::string &filename, - size_t maxIndex) { +std::map GTSAM_DEPRECATED +parse3DLandmarks(const std::string &filename, size_t maxIndex) { return parseVariables(filename, maxIndex); } #endif diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index ec5d6dce9d..db5d7d76ad 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -172,10 +172,6 @@ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); -/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel -GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, - const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); - /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, const noiseModel::Diagonal::shared_ptr model, @@ -504,17 +500,21 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -inline boost::optional parseVertex(std::istream &is, - const std::string &tag) { + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +inline boost::optional GTSAM_DEPRECATED +parseVertex(std::istream& is, const std::string& tag) { return parseVertexPose(is, tag); } -GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, - size_t maxIndex = 0); +GTSAM_EXPORT std::map GTSAM_DEPRECATED +parse3DPoses(const std::string& filename, size_t maxIndex = 0); -GTSAM_EXPORT std::map -parse3DLandmarks(const std::string &filename, size_t maxIndex = 0); +GTSAM_EXPORT std::map GTSAM_DEPRECATED +parse3DLandmarks(const std::string& filename, size_t maxIndex = 0); +GTSAM_EXPORT GraphAndValues GTSAM_DEPRECATED +load2D_robust(const std::string& filename, + const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); #endif } // namespace gtsam diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index bed11e5356..d87ca6f2d3 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -173,7 +173,7 @@ BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 typedef PriorFactor PriorFactorSimpleCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; From 906176291f7523534fdc864a98df3d14a9a80896 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 16:19:17 -0500 Subject: [PATCH 11/62] Fix everything to work with no deprecated methods allowed. --- gtsam/linear/GaussianConditional.cpp | 2 ++ gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/NoiseModel.h | 6 ++-- gtsam/navigation/AHRSFactor.cpp | 24 +++++++-------- gtsam/navigation/AHRSFactor.h | 36 ++++++++++------------- gtsam/navigation/tests/testAHRSFactor.cpp | 28 +++++++++--------- gtsam/navigation/tests/testImuBias.cpp | 8 ++--- 7 files changed, 48 insertions(+), 58 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 9297d6461b..e5016c6240 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -193,6 +193,7 @@ namespace gtsam { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { @@ -200,5 +201,6 @@ namespace gtsam { vectorPosition += getDim(frontal); } } +#endif } // namespace gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d93f65b425..958f411f86 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -128,7 +128,7 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ - void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; + void scaleFrontalsBySigma(VectorValues& gy) const; #endif private: diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index ba9c97efbd..5c379beb8d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -730,16 +730,14 @@ namespace gtsam { } // namespace noiseModel -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /** Note, deliberately not in noiseModel namespace. - * Deprecated. Only for compatibility with previous version. + /** + * Aliases. Deliberately not in noiseModel namespace. */ typedef noiseModel::Base::shared_ptr SharedNoiseModel; typedef noiseModel::Gaussian::shared_ptr SharedGaussian; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; -#endif /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 4604a55dd1..f4db42d0fd 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -168,13 +168,12 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, } //------------------------------------------------------------------------------ -Rot3 AHRSFactor::Predict( - const Rot3& rot_i, const Vector3& bias, - const PreintegratedAhrsMeasurements preintegratedMeasurements) { - const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); +Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements& pim) { + const Vector3 biascorrectedOmega = pim.predict(bias); // Coriolis term - const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i); + const Vector3 coriolis = pim.integrateCoriolis(rot_i); const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); @@ -184,27 +183,26 @@ Rot3 AHRSFactor::Predict( //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& pim, + const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, + bias), _PIM_(pim) { - boost::shared_ptr p = - boost::make_shared(pim.p()); + auto p = boost::make_shared(pim.p()); p->body_P_sensor = body_P_sensor; _PIM_.p_ = p; } //------------------------------------------------------------------------------ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements pim, + const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor) { - boost::shared_ptr p = - boost::make_shared(pim.p()); + auto p = boost::make_shared(pim.p()); p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; - PreintegratedMeasurements newPim = pim; + PreintegratedAhrsMeasurements newPim = pim; newPim.p_ = p; return Predict(rot_i, bias, newPim); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 94a9665542..10c33d101d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -104,15 +104,13 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @deprecated constructor - GTSAM_DEPRECATED PreintegratedAhrsMeasurements( - const Vector3& biasHat, const Matrix3& measuredOmegaCovariance) + /// @deprecated constructor, but used in tests. + PreintegratedAhrsMeasurements(const Vector3& biasHat, + const Matrix3& measuredOmegaCovariance) : PreintegratedRotation(boost::make_shared()), biasHat_(biasHat) { p_->gyroscopeCovariance = measuredOmegaCovariance; resetIntegration(); } -#endif private: @@ -183,27 +181,25 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { /// predicted states from IMU /// TODO(frank): relationship with PIM predict ?? - static Rot3 Predict( + static Rot3 Predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements& pim); + + /// @deprecated constructor, but used in tests. + AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedAhrsMeasurements& pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); + + /// @deprecated static function, but used in tests. + static Rot3 predict( const Rot3& rot_i, const Vector3& bias, - const PreintegratedAhrsMeasurements preintegratedMeasurements); + const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated name typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; - /// @deprecated constructor - GTSAM_DEPRECATED AHRSFactor( - Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor = boost::none); - - /// @deprecated static function - static Rot3 GTSAM_DEPRECATED - predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor = boost::none); #endif private: diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index a4d06d01a0..779f6abccf 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -54,11 +54,11 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3)); } -AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( +PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements( const Vector3& bias, const list& measuredOmegas, const list& deltaTs, const Vector3& initialRotationRate = Vector3::Zero()) { - AHRSFactor::PreintegratedMeasurements result(bias, I_3x3); + PreintegratedAhrsMeasurements result(bias, I_3x3); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); @@ -86,10 +86,10 @@ Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); } - } + //****************************************************************************** -TEST( AHRSFactor, PreintegratedMeasurements ) { +TEST( AHRSFactor, PreintegratedAhrsMeasurements ) { // Linearization point Vector3 bias(0,0,0); ///< Current estimate of angular rate bias @@ -102,7 +102,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { double expectedDeltaT1(0.5); // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3); + PreintegratedAhrsMeasurements actual1(bias, Z_3x3); actual1.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); @@ -113,7 +113,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { double expectedDeltaT2(1); // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements actual2 = actual1; + PreintegratedAhrsMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); @@ -159,7 +159,7 @@ TEST(AHRSFactor, Error) { Vector3 measuredOmega; measuredOmega << M_PI / 100, 0, 0; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3); + PreintegratedAhrsMeasurements pim(bias, Z_3x3); pim.integrateMeasurement(measuredOmega, deltaT); // Create factor @@ -217,7 +217,7 @@ TEST(AHRSFactor, ErrorWithBiases) { measuredOmega << 0, 0, M_PI / 10.0 + 0.3; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0), + PreintegratedAhrsMeasurements pim(Vector3(0,0,0), Z_3x3); pim.integrateMeasurement(measuredOmega, deltaT); @@ -360,7 +360,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { } // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements preintegrated = + PreintegratedAhrsMeasurements preintegrated = evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)); @@ -397,7 +397,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0)); - AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); pim.integrateMeasurement(measuredOmega, deltaT); @@ -439,7 +439,7 @@ TEST (AHRSFactor, predictTest) { Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.2; - AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance); for (int i = 0; i < 1000; ++i) { pim.integrateMeasurement(measuredOmega, deltaT); } @@ -456,9 +456,9 @@ TEST (AHRSFactor, predictTest) { Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); - // AHRSFactor::PreintegratedMeasurements::predict + // PreintegratedAhrsMeasurements::predict Matrix expectedH = numericalDerivative11( - std::bind(&AHRSFactor::PreintegratedMeasurements::predict, + std::bind(&PreintegratedAhrsMeasurements::predict, &pim, std::placeholders::_1, boost::none), bias); // Actual Jacobians @@ -478,7 +478,7 @@ TEST (AHRSFactor, graphTest) { // PreIntegrator Vector3 biasHat(0, 0, 0); - AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance); // Pre-integrate measurements Vector3 measuredOmega(0, M_PI / 20, 0); diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index b486a4a98b..81a1a2ceb9 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -47,20 +47,19 @@ TEST(ImuBias, Constructor) { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 TEST(ImuBias, inverse) { Bias biasActual = bias1.inverse(); Bias biasExpected = Bias(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, compose) { Bias biasActual = bias2.compose(bias1); Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, between) { // p.between(q) == q - p Bias biasActual = bias2.between(bias1); @@ -68,7 +67,6 @@ TEST(ImuBias, between) { EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, localCoordinates) { Vector deltaActual = Vector(bias2.localCoordinates(bias1)); Vector deltaExpected = @@ -76,7 +74,6 @@ TEST(ImuBias, localCoordinates) { EXPECT(assert_equal(deltaExpected, deltaActual)); } -/* ************************************************************************* */ TEST(ImuBias, retract) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; @@ -86,14 +83,12 @@ TEST(ImuBias, retract) { EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, Logmap) { Vector deltaActual = bias2.Logmap(bias1); Vector deltaExpected = bias1.vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } -/* ************************************************************************* */ TEST(ImuBias, Expmap) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; @@ -101,6 +96,7 @@ TEST(ImuBias, Expmap) { Bias biasExpected = Bias(delta); EXPECT(assert_equal(biasExpected, biasActual)); } +#endif /* ************************************************************************* */ TEST(ImuBias, operatorSub) { From a25e3f6d38f853d53fb6935511a74546efc9ba1d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 16:23:56 -0500 Subject: [PATCH 12/62] Remove deprecated methods from wrapper --- gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/linear.i | 1 - gtsam/navigation/navigation.i | 11 ----------- gtsam/slam/slam.i | 2 -- 4 files changed, 1 insertion(+), 15 deletions(-) diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 958f411f86..d93f65b425 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -128,7 +128,7 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ - void scaleFrontalsBySigma(VectorValues& gy) const; + void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; #endif private: diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 7b1ce550f0..7acc5bd416 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -467,7 +467,6 @@ virtual class GaussianConditional : gtsam::JacobianFactor { gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; Matrix R() const; Matrix S() const; Vector d() const; diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index c2a3bcdb42..6ede1645f3 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -18,23 +18,12 @@ class ConstantBias { // Group static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; // Operator Overloads gtsam::imuBias::ConstantBias operator-() const; gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - // Standard Interface Vector vector() const; Vector accelerometer() const; diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d276c4f2ec..a0a7329dd8 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -264,8 +264,6 @@ pair load2D( pair load2D( string filename, gtsam::noiseModel::Diagonal* model); pair load2D(string filename); -pair load2D_robust( - string filename, gtsam::noiseModel::Base* model, int maxIndex); void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); From 94f21358f4725b3ebc2afcfacf2a42ffe9b08358 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 13:31:06 -0500 Subject: [PATCH 13/62] fix decision tree equality and move default constructor to public --- gtsam/discrete/DecisionTree-inl.h | 2 +- gtsam/discrete/DecisionTree.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index f6a64f11fc..3bd2ac1137 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -79,7 +79,7 @@ namespace gtsam { bool equals(const Node& q, double tol) const override { const Leaf* other = dynamic_cast (&q); if (!other) return false; - return std::abs(double(this->constant_ - other->constant_)) < tol; + return this->constant_ == other->constant_; } /** print */ diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 0a78d46352..1e2c8b509e 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -113,14 +113,14 @@ namespace gtsam { convert(const typename DecisionTree::NodePtr& f, const std::map& map, std::function op); - /** Default constructor */ - DecisionTree(); - public: /// @name Standard Constructors /// @{ + /** Default constructor (for serialization) */ + DecisionTree(); + /** Create a constant */ DecisionTree(const Y& y); From ddaf9608d0855676306b379aa5c0d3cbeb4b7be6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 14:14:13 -0500 Subject: [PATCH 14/62] add formatting capabilities to DecisionTree --- gtsam/discrete/DecisionTree-inl.h | 20 +++++++++++--------- gtsam/discrete/DecisionTree.h | 18 +++++++++++++++--- gtsam/discrete/Potentials.cpp | 4 ++-- 3 files changed, 28 insertions(+), 14 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 3bd2ac1137..e2c0a944d1 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -83,7 +83,8 @@ namespace gtsam { } /** print */ - void print(const std::string& s) const override { + void print(const std::string& s, + const std::function formatter) const override { bool showZero = true; if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; } @@ -236,12 +237,11 @@ namespace gtsam { } /** print (as a tree) */ - void print(const std::string& s) const override { + void print(const std::string& s, const std::function formatter) const override { std::cout << s << " Choice("; - // std::cout << this << ","; - std::cout << label_ << ") " << std::endl; + std::cout << formatter(label_) << ") " << std::endl; for (size_t i = 0; i < branches_.size(); i++) - branches_[i]->print((boost::format("%s %d") % s % i).str()); + branches_[i]->print((boost::format("%s %d") % s % i).str(), formatter); } /** output to graphviz (as a a graph) */ @@ -591,7 +591,7 @@ namespace gtsam { // get new label M oldLabel = choice->label(); - L newLabel = map.at(oldLabel); + L newLabel = oldLabel; //map.at(oldLabel); // put together via Shannon expansion otherwise not sorted. std::vector functions; @@ -608,9 +608,11 @@ namespace gtsam { return root_->equals(*other.root_, tol); } - template - void DecisionTree::print(const std::string& s) const { - root_->print(s); + template + void DecisionTree::print( + const std::string& s, + const std::function formatter) const { + root_->print(s, formatter); } template diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 1e2c8b509e..68ddfa06b0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -20,13 +20,13 @@ #pragma once #include - #include #include #include #include #include +#include #include namespace gtsam { @@ -79,7 +79,13 @@ namespace gtsam { const void* id() const { return this; } // everything else is virtual, no documentation here as internal - virtual void print(const std::string& s = "") const = 0; + virtual void print( + const std::string& s = "", + const std::function formatter = [](const L& x) { + std::stringstream ss; + ss << x; + return ss.str(); + }) const = 0; virtual void dot(std::ostream& os, bool showZero) const = 0; virtual bool sameLeaf(const Leaf& q) const = 0; virtual bool sameLeaf(const Node& q) const = 0; @@ -154,7 +160,13 @@ namespace gtsam { /// @{ /** GTSAM-style print */ - void print(const std::string& s = "DecisionTree") const; + void print( + const std::string& s = "DecisionTree", + const std::function formatter = [](const L& x) { + std::stringstream ss; + ss << x; + return ss.str(); + }) const; // Testable bool equals(const DecisionTree& other, double tol = 1e-9) const; diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index fa491eba36..057b6a2655 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -51,11 +51,11 @@ bool Potentials::equals(const Potentials& other, double tol) const { /* ************************************************************************* */ void Potentials::print(const string& s, const KeyFormatter& formatter) const { - cout << s << "\n Cardinalities: {"; + cout << s << "\n Cardinalities: { "; for (const std::pair& key : cardinalities_) cout << formatter(key.first) << ":" << key.second << ", "; cout << "}" << endl; - ADT::print(" "); + ADT::print(" ", formatter); } // // /* ************************************************************************* */ From 8b5d93ad379367c941bf3351d8841286c4cbe0d7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 14:32:35 -0500 Subject: [PATCH 15/62] revert incorrect change --- gtsam/discrete/DecisionTree-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index e2c0a944d1..c26d254209 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -591,7 +591,7 @@ namespace gtsam { // get new label M oldLabel = choice->label(); - L newLabel = oldLabel; //map.at(oldLabel); + L newLabel = map.at(oldLabel); // put together via Shannon expansion otherwise not sorted. std::vector functions; From bb6e489c372cadcd50434731e4be5a4dcb6e5ac2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 15:15:19 -0500 Subject: [PATCH 16/62] new DecisionTree constructor and methods that takes an op to convert from one type to another # Conflicts: # gtsam/hybrid/DCMixtureFactor.h --- gtsam/discrete/DecisionTree-inl.h | 40 +++++++++++++++++++++++++++++++ gtsam/discrete/DecisionTree.h | 19 ++++++++++++++- 2 files changed, 58 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index c26d254209..099ccb5288 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -457,6 +457,14 @@ namespace gtsam { root_ = convert(other.root_, map, op); } + /*********************************************************************************/ + template + template + DecisionTree::DecisionTree(const DecisionTree& other, + std::function op) { + root_ = convert(other.root_, op); + } + /*********************************************************************************/ // Called by two constructors above. // Takes a label and a corresponding range of decision trees, and creates a new @@ -602,6 +610,38 @@ namespace gtsam { return LY::compose(functions.begin(), functions.end(), newLabel); } + /*********************************************************************************/ + template + template + typename DecisionTree::NodePtr DecisionTree::convert( + const typename DecisionTree::NodePtr& f, + std::function op) { + + typedef DecisionTree LX; + typedef typename LX::Leaf LXLeaf; + typedef typename LX::Choice LXChoice; + typedef typename LX::NodePtr LXNodePtr; + typedef DecisionTree LY; + + // ugliness below because apparently we can't have templated virtual functions + // If leaf, apply unary conversion "op" and create a unique leaf + const LXLeaf* leaf = dynamic_cast (f.get()); + if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); + + // Check if Choice + boost::shared_ptr choice = boost::dynamic_pointer_cast (f); + if (!choice) throw std::invalid_argument( + "DecisionTree::Convert: Invalid NodePtr"); + + // put together via Shannon expansion otherwise not sorted. + std::vector functions; + for(const LXNodePtr& branch: choice->branches()) { + LY converted(convert(branch, op)); + functions += converted; + } + return LY::compose(functions.begin(), functions.end(), choice->label()); + } + /*********************************************************************************/ template bool DecisionTree::equals(const DecisionTree& other, double tol) const { diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 68ddfa06b0..3b91def639 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -119,7 +119,12 @@ namespace gtsam { convert(const typename DecisionTree::NodePtr& f, const std::map& map, std::function op); - public: + /** Convert only node to a different type */ + template + NodePtr convert(const typename DecisionTree::NodePtr& f, + const std::function op); + + public: /// @name Standard Constructors /// @{ @@ -155,6 +160,11 @@ namespace gtsam { DecisionTree(const DecisionTree& other, const std::map& map, std::function op); + /** Convert only nodes from a different type */ + template + DecisionTree(const DecisionTree& other, + std::function op); + /// @} /// @name Testable /// @{ @@ -231,12 +241,19 @@ namespace gtsam { /** free versions of apply */ + //TODO(Varun) where are these templates Y, L and not L, Y? template DecisionTree apply(const DecisionTree& f, const typename DecisionTree::Unary& op) { return f.apply(op); } + template + DecisionTree apply(const DecisionTree& f, + const std::function& op) { + return f.apply(op); + } + template DecisionTree apply(const DecisionTree& f, const DecisionTree& g, From 315b10bb960fd765b24e78f87ae09e27a1127967 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 16:00:09 -0500 Subject: [PATCH 17/62] minor format --- gtsam/discrete/DecisionTree-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 099ccb5288..51d66c860d 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -77,7 +77,7 @@ namespace gtsam { /** equality up to tolerance */ bool equals(const Node& q, double tol) const override { - const Leaf* other = dynamic_cast (&q); + const Leaf* other = dynamic_cast(&q); if (!other) return false; return this->constant_ == other->constant_; } From 28071ed23dcd543283e41e925c83fdc4ea06028b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 20:43:16 -0500 Subject: [PATCH 18/62] added SFINAE methods for Leaf node equality checks --- gtsam/discrete/DecisionTree-inl.h | 38 +++++++++++++++++++++++++------ 1 file changed, 31 insertions(+), 7 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 51d66c860d..f3b0dbf3a0 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -19,22 +19,24 @@ #pragma once -#include #include +#include +#include +#include #include +#include #include #include -#include -using boost::assign::operator+=; +#include #include -#include - -#include #include #include +#include #include +using boost::assign::operator+=; + namespace gtsam { /*********************************************************************************/ @@ -75,11 +77,33 @@ namespace gtsam { return (q.isLeaf() && q.sameLeaf(*this)); } + /// @{ + /// SFINAE methods for proper substitution. + /** equality for integral types. */ + template + typename std::enable_if::value, bool>::type + equals(const T& a, const T& b, double tol) const { + return std::abs(double(a - b)) < tol; + } + /** equality for boost::shared_ptr types. */ + template + typename std::enable_if::value, bool>::type + equals(const T& a, const T& b, double tol) const { + return traits::Equals(*a, *b, tol); + } + /** equality for all other types. */ + template + typename std::enable_if::value && !std::is_integral::value, bool>::type + equals(const Y& a, const Y& b, double tol) const { + return traits::Equals(a, b, tol); + } + /// @} + /** equality up to tolerance */ bool equals(const Node& q, double tol) const override { const Leaf* other = dynamic_cast(&q); if (!other) return false; - return this->constant_ == other->constant_; + return this->equals(this->constant_, other->constant_, tol); } /** print */ From f1dedca2b791f9355130086c3185721d5fdeff18 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 20:44:35 -0500 Subject: [PATCH 19/62] replace dot with DOT to prevent collision with vector dot product --- .../tests/testAlgebraicDecisionTree.cpp | 90 +++++++++---------- gtsam/discrete/tests/testDecisionTree.cpp | 10 +-- 2 files changed, 49 insertions(+), 51 deletions(-) diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 7a33810c7d..becc5a2a13 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -48,7 +48,7 @@ template<> struct traits : public Testable {}; #define DISABLE_DOT template -void dot(const T&f, const string& filename) { +void DOT(const T&f, const string& filename) { #ifndef DISABLE_DOT f.dot(filename); #endif @@ -112,7 +112,7 @@ TEST(ADT, example3) ADT note(E, 0.9, 0.1); ADT cnotb = c * notb; - dot(cnotb, "ADT-cnotb"); + DOT(cnotb, "ADT-cnotb"); // a.print("a: "); // cnotb.print("cnotb: "); @@ -120,11 +120,11 @@ TEST(ADT, example3) // acnotb.print("acnotb: "); // acnotb.printCache("acnotb Cache:"); - dot(acnotb, "ADT-acnotb"); + DOT(acnotb, "ADT-acnotb"); ADT big = apply(apply(d, note, &mul), acnotb, &add_); - dot(big, "ADT-big"); + DOT(big, "ADT-big"); } /* ******************************************************************************** */ @@ -136,8 +136,8 @@ ADT create(const Signature& signature) { ADT p(signature.discreteKeys(), signature.cpt()); static size_t count = 0; const DiscreteKey& key = signature.key(); - string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); - dot(p, dotfile); + string DOTfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); + DOT(p, DOTfile); return p; } @@ -167,21 +167,21 @@ TEST(ADT, joint) resetCounts(); gttic_(asiaJoint); ADT joint = pA; - dot(joint, "Asia-A"); + DOT(joint, "Asia-A"); joint = apply(joint, pS, &mul); - dot(joint, "Asia-AS"); + DOT(joint, "Asia-AS"); joint = apply(joint, pT, &mul); - dot(joint, "Asia-AST"); + DOT(joint, "Asia-AST"); joint = apply(joint, pL, &mul); - dot(joint, "Asia-ASTL"); + DOT(joint, "Asia-ASTL"); joint = apply(joint, pB, &mul); - dot(joint, "Asia-ASTLB"); + DOT(joint, "Asia-ASTLB"); joint = apply(joint, pE, &mul); - dot(joint, "Asia-ASTLBE"); + DOT(joint, "Asia-ASTLBE"); joint = apply(joint, pX, &mul); - dot(joint, "Asia-ASTLBEX"); + DOT(joint, "Asia-ASTLBEX"); joint = apply(joint, pD, &mul); - dot(joint, "Asia-ASTLBEXD"); + DOT(joint, "Asia-ASTLBEXD"); EXPECT_LONGS_EQUAL(346, muls); gttoc_(asiaJoint); tictoc_getNode(asiaJointNode, asiaJoint); @@ -229,21 +229,21 @@ TEST(ADT, inference) resetCounts(); gttic_(asiaProd); ADT joint = pA; - dot(joint, "Joint-Product-A"); + DOT(joint, "Joint-Product-A"); joint = apply(joint, pS, &mul); - dot(joint, "Joint-Product-AS"); + DOT(joint, "Joint-Product-AS"); joint = apply(joint, pT, &mul); - dot(joint, "Joint-Product-AST"); + DOT(joint, "Joint-Product-AST"); joint = apply(joint, pL, &mul); - dot(joint, "Joint-Product-ASTL"); + DOT(joint, "Joint-Product-ASTL"); joint = apply(joint, pB, &mul); - dot(joint, "Joint-Product-ASTLB"); + DOT(joint, "Joint-Product-ASTLB"); joint = apply(joint, pE, &mul); - dot(joint, "Joint-Product-ASTLBE"); + DOT(joint, "Joint-Product-ASTLBE"); joint = apply(joint, pX, &mul); - dot(joint, "Joint-Product-ASTLBEX"); + DOT(joint, "Joint-Product-ASTLBEX"); joint = apply(joint, pD, &mul); - dot(joint, "Joint-Product-ASTLBEXD"); + DOT(joint, "Joint-Product-ASTLBEXD"); EXPECT_LONGS_EQUAL(370, (long)muls); // different ordering gttoc_(asiaProd); tictoc_getNode(asiaProdNode, asiaProd); @@ -255,13 +255,13 @@ TEST(ADT, inference) gttic_(asiaSum); ADT marginal = joint; marginal = marginal.combine(X, &add_); - dot(marginal, "Joint-Sum-ADBLEST"); + DOT(marginal, "Joint-Sum-ADBLEST"); marginal = marginal.combine(T, &add_); - dot(marginal, "Joint-Sum-ADBLES"); + DOT(marginal, "Joint-Sum-ADBLES"); marginal = marginal.combine(S, &add_); - dot(marginal, "Joint-Sum-ADBLE"); + DOT(marginal, "Joint-Sum-ADBLE"); marginal = marginal.combine(E, &add_); - dot(marginal, "Joint-Sum-ADBL"); + DOT(marginal, "Joint-Sum-ADBL"); EXPECT_LONGS_EQUAL(161, (long)adds); gttoc_(asiaSum); tictoc_getNode(asiaSumNode, asiaSum); @@ -300,7 +300,7 @@ TEST(ADT, factor_graph) fg = apply(fg, pE, &mul); fg = apply(fg, pX, &mul); fg = apply(fg, pD, &mul); - dot(fg, "FactorGraph"); + DOT(fg, "FactorGraph"); EXPECT_LONGS_EQUAL(158, (long)muls); gttoc_(asiaFG); tictoc_getNode(asiaFGNode, asiaFG); @@ -311,15 +311,15 @@ TEST(ADT, factor_graph) resetCounts(); gttic_(marg); fg = fg.combine(X, &add_); - dot(fg, "Marginalized-6X"); + DOT(fg, "Marginalized-6X"); fg = fg.combine(T, &add_); - dot(fg, "Marginalized-5T"); + DOT(fg, "Marginalized-5T"); fg = fg.combine(S, &add_); - dot(fg, "Marginalized-4S"); + DOT(fg, "Marginalized-4S"); fg = fg.combine(E, &add_); - dot(fg, "Marginalized-3E"); + DOT(fg, "Marginalized-3E"); fg = fg.combine(L, &add_); - dot(fg, "Marginalized-2L"); + DOT(fg, "Marginalized-2L"); EXPECT(adds = 54); gttoc_(marg); tictoc_getNode(margNode, marg); @@ -333,9 +333,9 @@ TEST(ADT, factor_graph) resetCounts(); gttic_(elimX); ADT fE = pX; - dot(fE, "Eliminate-01-fEX"); + DOT(fE, "Eliminate-01-fEX"); fE = fE.combine(X, &add_); - dot(fE, "Eliminate-02-fE"); + DOT(fE, "Eliminate-02-fE"); gttoc_(elimX); tictoc_getNode(elimXNode, elimX); elapsed = elimXNode->secs() + elimXNode->wall(); @@ -347,9 +347,9 @@ TEST(ADT, factor_graph) gttic_(elimT); ADT fLE = pT; fLE = apply(fLE, pE, &mul); - dot(fLE, "Eliminate-03-fLET"); + DOT(fLE, "Eliminate-03-fLET"); fLE = fLE.combine(T, &add_); - dot(fLE, "Eliminate-04-fLE"); + DOT(fLE, "Eliminate-04-fLE"); gttoc_(elimT); tictoc_getNode(elimTNode, elimT); elapsed = elimTNode->secs() + elimTNode->wall(); @@ -362,9 +362,9 @@ TEST(ADT, factor_graph) ADT fBL = pS; fBL = apply(fBL, pL, &mul); fBL = apply(fBL, pB, &mul); - dot(fBL, "Eliminate-05-fBLS"); + DOT(fBL, "Eliminate-05-fBLS"); fBL = fBL.combine(S, &add_); - dot(fBL, "Eliminate-06-fBL"); + DOT(fBL, "Eliminate-06-fBL"); gttoc_(elimS); tictoc_getNode(elimSNode, elimS); elapsed = elimSNode->secs() + elimSNode->wall(); @@ -377,9 +377,9 @@ TEST(ADT, factor_graph) ADT fBL2 = fE; fBL2 = apply(fBL2, fLE, &mul); fBL2 = apply(fBL2, pD, &mul); - dot(fBL2, "Eliminate-07-fBLE"); + DOT(fBL2, "Eliminate-07-fBLE"); fBL2 = fBL2.combine(E, &add_); - dot(fBL2, "Eliminate-08-fBL2"); + DOT(fBL2, "Eliminate-08-fBL2"); gttoc_(elimE); tictoc_getNode(elimENode, elimE); elapsed = elimENode->secs() + elimENode->wall(); @@ -391,9 +391,9 @@ TEST(ADT, factor_graph) gttic_(elimL); ADT fB = fBL; fB = apply(fB, fBL2, &mul); - dot(fB, "Eliminate-09-fBL"); + DOT(fB, "Eliminate-09-fBL"); fB = fB.combine(L, &add_); - dot(fB, "Eliminate-10-fB"); + DOT(fB, "Eliminate-10-fB"); gttoc_(elimL); tictoc_getNode(elimLNode, elimL); elapsed = elimLNode->secs() + elimLNode->wall(); @@ -491,7 +491,7 @@ TEST(ADT, conversion) { DiscreteKey X(0,2), Y(1,2); ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); - dot(fDiscreteKey, "conversion-f1"); + DOT(fDiscreteKey, "conversion-f1"); std::map keyMap; keyMap[0] = 5; @@ -500,7 +500,7 @@ TEST(ADT, conversion) AlgebraicDecisionTree fIndexKey(fDiscreteKey, keyMap); // f1.print("f1"); // f2.print("f2"); - dot(fIndexKey, "conversion-f2"); + DOT(fIndexKey, "conversion-f2"); DiscreteValues x00, x01, x02, x10, x11, x12; x00[5] = 0, x00[2] = 0; @@ -519,7 +519,7 @@ TEST(ADT, elimination) { DiscreteKey A(0,2), B(1,3), C(2,2); ADT f1(A & B & C, "1 2 3 4 5 6 1 8 3 3 5 5"); - dot(f1, "elimination-f1"); + DOT(f1, "elimination-f1"); { // sum out lower key diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 96f503abc9..9eb06f2c4f 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -32,14 +32,12 @@ using namespace std; using namespace gtsam; template -void dot(const T&f, const string& filename) { +void DOT(const T&f, const string& filename) { #ifndef DISABLE_DOT f.dot(filename); #endif } -#define DOT(x)(dot(x,#x)) - struct Crazy { int a; double b; }; typedef DecisionTree CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be) @@ -179,8 +177,8 @@ TEST(DT, example) enum Label { U, V, X, Y, Z }; -typedef DecisionTree BDT; -bool convert(const int& y) { +typedef DecisionTree BDT; +int convert(const int& y) { return y != 0; } @@ -196,7 +194,7 @@ TEST(DT, conversion) map ordering; ordering[A] = X; ordering[B] = Y; - std::function op = convert; + std::function op = convert; BDT f2(f1, ordering, op); // f1.print("f1"); // f2.print("f2"); From 1c76de40d1e4ee8d07aa3723a8931b2e18c0f626 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 20:45:55 -0500 Subject: [PATCH 20/62] minor fix --- gtsam/discrete/tests/testDecisionTree.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 9eb06f2c4f..28b8866add 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -32,12 +32,14 @@ using namespace std; using namespace gtsam; template -void DOT(const T&f, const string& filename) { +void write_dot(const T&f, const string& filename) { #ifndef DISABLE_DOT f.dot(filename); #endif } +#define DOT(x)(write_dot(x,#x)) + struct Crazy { int a; double b; }; typedef DecisionTree CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be) From 573d0d17737cf11edb0d24b7677a2e964523775b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 23:46:20 -0500 Subject: [PATCH 21/62] undo change to test --- gtsam/discrete/tests/testDecisionTree.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 28b8866add..b44306723c 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -179,8 +179,8 @@ TEST(DT, example) enum Label { U, V, X, Y, Z }; -typedef DecisionTree BDT; -int convert(const int& y) { +typedef DecisionTree BDT; +bool convert(const int& y) { return y != 0; } @@ -196,7 +196,7 @@ TEST(DT, conversion) map ordering; ordering[A] = X; ordering[B] = Y; - std::function op = convert; + std::function op = convert; BDT f2(f1, ordering, op); // f1.print("f1"); // f2.print("f2"); From ed839083e214a2d834d25230e3a877cbd94c6a49 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 29 Dec 2021 23:47:20 -0500 Subject: [PATCH 22/62] formatter passed as reference and added a default formatter method --- gtsam/discrete/DecisionTree-inl.h | 7 ++++--- gtsam/discrete/DecisionTree.h | 34 +++++++++++++++---------------- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index f3b0dbf3a0..ec6222fd74 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -108,7 +108,7 @@ namespace gtsam { /** print */ void print(const std::string& s, - const std::function formatter) const override { + const std::function& formatter) const override { bool showZero = true; if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; } @@ -261,7 +261,8 @@ namespace gtsam { } /** print (as a tree) */ - void print(const std::string& s, const std::function formatter) const override { + void print(const std::string& s, + const std::function& formatter) const override { std::cout << s << " Choice("; std::cout << formatter(label_) << ") " << std::endl; for (size_t i = 0; i < branches_.size(); i++) @@ -675,7 +676,7 @@ namespace gtsam { template void DecisionTree::print( const std::string& s, - const std::function formatter) const { + const std::function& formatter) const { root_->print(s, formatter); } diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 3b91def639..498fce5aa7 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -39,6 +39,13 @@ namespace gtsam { template class GTSAM_EXPORT DecisionTree { + /// default method used by `formatter` when printing. + static std::string DefaultFormatter(const L& x) { + std::stringstream ss; + ss << x; + return ss.str(); + } + public: /** Handy typedefs for unary and binary function types */ @@ -79,13 +86,9 @@ namespace gtsam { const void* id() const { return this; } // everything else is virtual, no documentation here as internal - virtual void print( - const std::string& s = "", - const std::function formatter = [](const L& x) { - std::stringstream ss; - ss << x; - return ss.str(); - }) const = 0; + virtual void print(const std::string& s = "", + const std::function& formatter = + &DefaultFormatter) const = 0; virtual void dot(std::ostream& os, bool showZero) const = 0; virtual bool sameLeaf(const Leaf& q) const = 0; virtual bool sameLeaf(const Node& q) const = 0; @@ -170,13 +173,9 @@ namespace gtsam { /// @{ /** GTSAM-style print */ - void print( - const std::string& s = "DecisionTree", - const std::function formatter = [](const L& x) { - std::stringstream ss; - ss << x; - return ss.str(); - }) const; + void print(const std::string& s = "DecisionTree", + const std::function& formatter = + &DefaultFormatter) const; // Testable bool equals(const DecisionTree& other, double tol = 1e-9) const; @@ -241,20 +240,19 @@ namespace gtsam { /** free versions of apply */ - //TODO(Varun) where are these templates Y, L and not L, Y? - template + template DecisionTree apply(const DecisionTree& f, const typename DecisionTree::Unary& op) { return f.apply(op); } - template + template DecisionTree apply(const DecisionTree& f, const std::function& op) { return f.apply(op); } - template + template DecisionTree apply(const DecisionTree& f, const DecisionTree& g, const typename DecisionTree::Binary& op) { From 9982057a2b832ab0ac0ce348eeb8bcd0156ebd9c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Dec 2021 11:32:51 -0500 Subject: [PATCH 23/62] undo dot changes --- .../tests/testAlgebraicDecisionTree.cpp | 96 +++++++++---------- gtsam/discrete/tests/testDecisionTree.cpp | 4 +- 2 files changed, 50 insertions(+), 50 deletions(-) diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index becc5a2a13..910515b5c4 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -48,7 +48,7 @@ template<> struct traits : public Testable {}; #define DISABLE_DOT template -void DOT(const T&f, const string& filename) { +void dot(const T&f, const string& filename) { #ifndef DISABLE_DOT f.dot(filename); #endif @@ -112,7 +112,7 @@ TEST(ADT, example3) ADT note(E, 0.9, 0.1); ADT cnotb = c * notb; - DOT(cnotb, "ADT-cnotb"); + dot(cnotb, "ADT-cnotb"); // a.print("a: "); // cnotb.print("cnotb: "); @@ -120,11 +120,11 @@ TEST(ADT, example3) // acnotb.print("acnotb: "); // acnotb.printCache("acnotb Cache:"); - DOT(acnotb, "ADT-acnotb"); + dot(acnotb, "ADT-acnotb"); ADT big = apply(apply(d, note, &mul), acnotb, &add_); - DOT(big, "ADT-big"); + dot(big, "ADT-big"); } /* ******************************************************************************** */ @@ -137,7 +137,7 @@ ADT create(const Signature& signature) { static size_t count = 0; const DiscreteKey& key = signature.key(); string DOTfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); - DOT(p, DOTfile); + dot(p, DOTfile); return p; } @@ -167,21 +167,21 @@ TEST(ADT, joint) resetCounts(); gttic_(asiaJoint); ADT joint = pA; - DOT(joint, "Asia-A"); + dot(joint, "Asia-A"); joint = apply(joint, pS, &mul); - DOT(joint, "Asia-AS"); + dot(joint, "Asia-AS"); joint = apply(joint, pT, &mul); - DOT(joint, "Asia-AST"); + dot(joint, "Asia-AST"); joint = apply(joint, pL, &mul); - DOT(joint, "Asia-ASTL"); + dot(joint, "Asia-ASTL"); joint = apply(joint, pB, &mul); - DOT(joint, "Asia-ASTLB"); + dot(joint, "Asia-ASTLB"); joint = apply(joint, pE, &mul); - DOT(joint, "Asia-ASTLBE"); + dot(joint, "Asia-ASTLBE"); joint = apply(joint, pX, &mul); - DOT(joint, "Asia-ASTLBEX"); + dot(joint, "Asia-ASTLBEX"); joint = apply(joint, pD, &mul); - DOT(joint, "Asia-ASTLBEXD"); + dot(joint, "Asia-ASTLBEXD"); EXPECT_LONGS_EQUAL(346, muls); gttoc_(asiaJoint); tictoc_getNode(asiaJointNode, asiaJoint); @@ -229,21 +229,21 @@ TEST(ADT, inference) resetCounts(); gttic_(asiaProd); ADT joint = pA; - DOT(joint, "Joint-Product-A"); + dot(joint, "Joint-Product-A"); joint = apply(joint, pS, &mul); - DOT(joint, "Joint-Product-AS"); + dot(joint, "Joint-Product-AS"); joint = apply(joint, pT, &mul); - DOT(joint, "Joint-Product-AST"); + dot(joint, "Joint-Product-AST"); joint = apply(joint, pL, &mul); - DOT(joint, "Joint-Product-ASTL"); + dot(joint, "Joint-Product-ASTL"); joint = apply(joint, pB, &mul); - DOT(joint, "Joint-Product-ASTLB"); + dot(joint, "Joint-Product-ASTLB"); joint = apply(joint, pE, &mul); - DOT(joint, "Joint-Product-ASTLBE"); + dot(joint, "Joint-Product-ASTLBE"); joint = apply(joint, pX, &mul); - DOT(joint, "Joint-Product-ASTLBEX"); + dot(joint, "Joint-Product-ASTLBEX"); joint = apply(joint, pD, &mul); - DOT(joint, "Joint-Product-ASTLBEXD"); + dot(joint, "Joint-Product-ASTLBEXD"); EXPECT_LONGS_EQUAL(370, (long)muls); // different ordering gttoc_(asiaProd); tictoc_getNode(asiaProdNode, asiaProd); @@ -255,13 +255,13 @@ TEST(ADT, inference) gttic_(asiaSum); ADT marginal = joint; marginal = marginal.combine(X, &add_); - DOT(marginal, "Joint-Sum-ADBLEST"); + dot(marginal, "Joint-Sum-ADBLEST"); marginal = marginal.combine(T, &add_); - DOT(marginal, "Joint-Sum-ADBLES"); + dot(marginal, "Joint-Sum-ADBLES"); marginal = marginal.combine(S, &add_); - DOT(marginal, "Joint-Sum-ADBLE"); + dot(marginal, "Joint-Sum-ADBLE"); marginal = marginal.combine(E, &add_); - DOT(marginal, "Joint-Sum-ADBL"); + dot(marginal, "Joint-Sum-ADBL"); EXPECT_LONGS_EQUAL(161, (long)adds); gttoc_(asiaSum); tictoc_getNode(asiaSumNode, asiaSum); @@ -300,7 +300,7 @@ TEST(ADT, factor_graph) fg = apply(fg, pE, &mul); fg = apply(fg, pX, &mul); fg = apply(fg, pD, &mul); - DOT(fg, "FactorGraph"); + dot(fg, "FactorGraph"); EXPECT_LONGS_EQUAL(158, (long)muls); gttoc_(asiaFG); tictoc_getNode(asiaFGNode, asiaFG); @@ -311,15 +311,15 @@ TEST(ADT, factor_graph) resetCounts(); gttic_(marg); fg = fg.combine(X, &add_); - DOT(fg, "Marginalized-6X"); + dot(fg, "Marginalized-6X"); fg = fg.combine(T, &add_); - DOT(fg, "Marginalized-5T"); + dot(fg, "Marginalized-5T"); fg = fg.combine(S, &add_); - DOT(fg, "Marginalized-4S"); + dot(fg, "Marginalized-4S"); fg = fg.combine(E, &add_); - DOT(fg, "Marginalized-3E"); + dot(fg, "Marginalized-3E"); fg = fg.combine(L, &add_); - DOT(fg, "Marginalized-2L"); + dot(fg, "Marginalized-2L"); EXPECT(adds = 54); gttoc_(marg); tictoc_getNode(margNode, marg); @@ -333,9 +333,9 @@ TEST(ADT, factor_graph) resetCounts(); gttic_(elimX); ADT fE = pX; - DOT(fE, "Eliminate-01-fEX"); + dot(fE, "Eliminate-01-fEX"); fE = fE.combine(X, &add_); - DOT(fE, "Eliminate-02-fE"); + dot(fE, "Eliminate-02-fE"); gttoc_(elimX); tictoc_getNode(elimXNode, elimX); elapsed = elimXNode->secs() + elimXNode->wall(); @@ -347,9 +347,9 @@ TEST(ADT, factor_graph) gttic_(elimT); ADT fLE = pT; fLE = apply(fLE, pE, &mul); - DOT(fLE, "Eliminate-03-fLET"); + dot(fLE, "Eliminate-03-fLET"); fLE = fLE.combine(T, &add_); - DOT(fLE, "Eliminate-04-fLE"); + dot(fLE, "Eliminate-04-fLE"); gttoc_(elimT); tictoc_getNode(elimTNode, elimT); elapsed = elimTNode->secs() + elimTNode->wall(); @@ -362,9 +362,9 @@ TEST(ADT, factor_graph) ADT fBL = pS; fBL = apply(fBL, pL, &mul); fBL = apply(fBL, pB, &mul); - DOT(fBL, "Eliminate-05-fBLS"); + dot(fBL, "Eliminate-05-fBLS"); fBL = fBL.combine(S, &add_); - DOT(fBL, "Eliminate-06-fBL"); + dot(fBL, "Eliminate-06-fBL"); gttoc_(elimS); tictoc_getNode(elimSNode, elimS); elapsed = elimSNode->secs() + elimSNode->wall(); @@ -377,9 +377,9 @@ TEST(ADT, factor_graph) ADT fBL2 = fE; fBL2 = apply(fBL2, fLE, &mul); fBL2 = apply(fBL2, pD, &mul); - DOT(fBL2, "Eliminate-07-fBLE"); + dot(fBL2, "Eliminate-07-fBLE"); fBL2 = fBL2.combine(E, &add_); - DOT(fBL2, "Eliminate-08-fBL2"); + dot(fBL2, "Eliminate-08-fBL2"); gttoc_(elimE); tictoc_getNode(elimENode, elimE); elapsed = elimENode->secs() + elimENode->wall(); @@ -391,9 +391,9 @@ TEST(ADT, factor_graph) gttic_(elimL); ADT fB = fBL; fB = apply(fB, fBL2, &mul); - DOT(fB, "Eliminate-09-fBL"); + dot(fB, "Eliminate-09-fBL"); fB = fB.combine(L, &add_); - DOT(fB, "Eliminate-10-fB"); + dot(fB, "Eliminate-10-fB"); gttoc_(elimL); tictoc_getNode(elimLNode, elimL); elapsed = elimLNode->secs() + elimLNode->wall(); @@ -414,13 +414,13 @@ TEST(ADT, equality_noparser) // Check straight equality ADT pA1 = create(A % tableA); ADT pA2 = create(A % tableA); - EXPECT(pA1 == pA2); // should be equal + EXPECT(pA1.equals(pA2)); // should be equal // Check equality after apply ADT pB = create(B % tableB); ADT pAB1 = apply(pA1, pB, &mul); ADT pAB2 = apply(pB, pA1, &mul); - EXPECT(pAB2 == pAB1); + EXPECT(pAB2.equals(pAB1)); } /* ************************************************************************* */ @@ -431,13 +431,13 @@ TEST(ADT, equality_parser) // Check straight equality ADT pA1 = create(A % "80/20"); ADT pA2 = create(A % "80/20"); - EXPECT(pA1 == pA2); // should be equal + EXPECT(pA1.equals(pA2)); // should be equal // Check equality after apply ADT pB = create(B % "60/40"); ADT pAB1 = apply(pA1, pB, &mul); ADT pAB2 = apply(pB, pA1, &mul); - EXPECT(pAB2 == pAB1); + EXPECT(pAB2.equals(pAB1)); } /* ******************************************************************************** */ @@ -491,7 +491,7 @@ TEST(ADT, conversion) { DiscreteKey X(0,2), Y(1,2); ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); - DOT(fDiscreteKey, "conversion-f1"); + dot(fDiscreteKey, "conversion-f1"); std::map keyMap; keyMap[0] = 5; @@ -500,7 +500,7 @@ TEST(ADT, conversion) AlgebraicDecisionTree fIndexKey(fDiscreteKey, keyMap); // f1.print("f1"); // f2.print("f2"); - DOT(fIndexKey, "conversion-f2"); + dot(fIndexKey, "conversion-f2"); DiscreteValues x00, x01, x02, x10, x11, x12; x00[5] = 0, x00[2] = 0; @@ -519,7 +519,7 @@ TEST(ADT, elimination) { DiscreteKey A(0,2), B(1,3), C(2,2); ADT f1(A & B & C, "1 2 3 4 5 6 1 8 3 3 5 5"); - DOT(f1, "elimination-f1"); + dot(f1, "elimination-f1"); { // sum out lower key diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index b44306723c..96f503abc9 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -32,13 +32,13 @@ using namespace std; using namespace gtsam; template -void write_dot(const T&f, const string& filename) { +void dot(const T&f, const string& filename) { #ifndef DISABLE_DOT f.dot(filename); #endif } -#define DOT(x)(write_dot(x,#x)) +#define DOT(x)(dot(x,#x)) struct Crazy { int a; double b; }; typedef DecisionTree CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be) From b24da8399a363e27be3d281c5aed39aa11e07d57 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Dec 2021 11:34:05 -0500 Subject: [PATCH 24/62] add comparator as argument to equals method # Conflicts: # gtsam/hybrid/DCMixtureFactor.h --- gtsam/discrete/AlgebraicDecisionTree.h | 13 +++++++ gtsam/discrete/DecisionTree-inl.h | 46 ++++++++--------------- gtsam/discrete/DecisionTree.h | 15 ++++++-- gtsam/discrete/tests/testDecisionTree.cpp | 14 ++++++- 4 files changed, 53 insertions(+), 35 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 9cc55ed6aa..3469ac5cf6 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -30,6 +30,13 @@ namespace gtsam { template class AlgebraicDecisionTree: public DecisionTree { + /** + * @brief Default method for comparison of two doubles upto some tolerance. + */ + static bool DefaultComparator(double a, double b, double tol) { + return std::abs(a - b) < tol; + } + public: typedef DecisionTree Super; @@ -138,6 +145,12 @@ namespace gtsam { return this->combine(labelC, &Ring::add); } + /// Equality method customized to node type `double`. + bool equals(const AlgebraicDecisionTree& other, double tol = 1e-9, + const std::function& comparator = + &DefaultComparator) const { + return this->root_->equals(*other.root_, tol, comparator); + } }; // AlgebraicDecisionTree diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index ec6222fd74..f531e2b983 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -20,7 +20,6 @@ #pragma once #include -#include #include #include @@ -77,33 +76,13 @@ namespace gtsam { return (q.isLeaf() && q.sameLeaf(*this)); } - /// @{ - /// SFINAE methods for proper substitution. - /** equality for integral types. */ - template - typename std::enable_if::value, bool>::type - equals(const T& a, const T& b, double tol) const { - return std::abs(double(a - b)) < tol; - } - /** equality for boost::shared_ptr types. */ - template - typename std::enable_if::value, bool>::type - equals(const T& a, const T& b, double tol) const { - return traits::Equals(*a, *b, tol); - } - /** equality for all other types. */ - template - typename std::enable_if::value && !std::is_integral::value, bool>::type - equals(const Y& a, const Y& b, double tol) const { - return traits::Equals(a, b, tol); - } - /// @} - /** equality up to tolerance */ - bool equals(const Node& q, double tol) const override { + bool equals(const Node& q, double tol, + const std::function& + comparator) const override { const Leaf* other = dynamic_cast(&q); if (!other) return false; - return this->equals(this->constant_, other->constant_, tol); + return comparator(this->constant_, other->constant_, tol); } /** print */ @@ -304,14 +283,17 @@ namespace gtsam { } /** equality up to tolerance */ - bool equals(const Node& q, double tol) const override { - const Choice* other = dynamic_cast (&q); + bool equals(const Node& q, double tol, + const std::function& + comparator) const override { + const Choice* other = dynamic_cast(&q); if (!other) return false; if (this->label_ != other->label_) return false; if (branches_.size() != other->branches_.size()) return false; // we don't care about shared pointers being equal here for (size_t i = 0; i < branches_.size(); i++) - if (!(branches_[i]->equals(*(other->branches_[i]), tol))) return false; + if (!(branches_[i]->equals(*(other->branches_[i]), tol, comparator))) + return false; return true; } @@ -668,9 +650,11 @@ namespace gtsam { } /*********************************************************************************/ - template - bool DecisionTree::equals(const DecisionTree& other, double tol) const { - return root_->equals(*other.root_, tol); + template + bool DecisionTree::equals( + const DecisionTree& other, double tol, + const std::function& comparator) const { + return root_->equals(*other.root_, tol, comparator); } template diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 498fce5aa7..eb23bc5ce0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -39,13 +39,18 @@ namespace gtsam { template class GTSAM_EXPORT DecisionTree { - /// default method used by `formatter` when printing. + /// Default method used by `formatter` when printing. static std::string DefaultFormatter(const L& x) { std::stringstream ss; ss << x; return ss.str(); } + /// Default method for comparison of two objects of type Y. + static bool DefaultComparator(const Y& a, const Y& b, double tol) { + return a == b; + } + public: /** Handy typedefs for unary and binary function types */ @@ -92,7 +97,9 @@ namespace gtsam { virtual void dot(std::ostream& os, bool showZero) const = 0; virtual bool sameLeaf(const Leaf& q) const = 0; virtual bool sameLeaf(const Node& q) const = 0; - virtual bool equals(const Node& other, double tol = 1e-9) const = 0; + virtual bool equals(const Node& other, double tol = 1e-9, + const std::function& + comparator = &DefaultComparator) const = 0; virtual const Y& operator()(const Assignment& x) const = 0; virtual Ptr apply(const Unary& op) const = 0; virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; @@ -178,7 +185,9 @@ namespace gtsam { &DefaultFormatter) const; // Testable - bool equals(const DecisionTree& other, double tol = 1e-9) const; + bool equals(const DecisionTree& other, double tol = 1e-9, + const std::function& + comparator = &DefaultComparator) const; /// @} /// @name Standard Interface diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 96f503abc9..dbf6cc44b5 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -40,7 +40,19 @@ void dot(const T&f, const string& filename) { #define DOT(x)(dot(x,#x)) -struct Crazy { int a; double b; }; +struct Crazy { + int a; + double b; + + bool equals(const Crazy& other, double tol = 1e-12) const { + return a == other.a && std::abs(b - other.b) < tol; + } + + bool operator==(const Crazy& other) const { + return this->equals(other); + } +}; + typedef DecisionTree CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be) // traits From 26c48a8c1fc33f9bcebfbcfd0c813ea30c551811 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Dec 2021 15:28:07 -0500 Subject: [PATCH 25/62] address more review comments # Conflicts: # gtsam/hybrid/DCGaussianMixtureFactor.h # gtsam/hybrid/DCMixtureFactor.h --- gtsam/discrete/AlgebraicDecisionTree.h | 17 ++++++----------- gtsam/discrete/DecisionTree-inl.h | 21 ++++++++------------- gtsam/discrete/DecisionTree.h | 21 +++++++++++---------- 3 files changed, 25 insertions(+), 34 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 3469ac5cf6..f47e01668c 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -30,14 +30,7 @@ namespace gtsam { template class AlgebraicDecisionTree: public DecisionTree { - /** - * @brief Default method for comparison of two doubles upto some tolerance. - */ - static bool DefaultComparator(double a, double b, double tol) { - return std::abs(a - b) < tol; - } - - public: + public: typedef DecisionTree Super; @@ -146,9 +139,11 @@ namespace gtsam { } /// Equality method customized to node type `double`. - bool equals(const AlgebraicDecisionTree& other, double tol = 1e-9, - const std::function& comparator = - &DefaultComparator) const { + bool equals(const AlgebraicDecisionTree& other, double tol = 1e-9) const { + // lambda for comparison of two doubles upto some tolerance. + auto comparator = [](double a, double b, double tol) { + return std::abs(a - b) < tol; + }; return this->root_->equals(*other.root_, tol, comparator); } }; diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index f531e2b983..fb6c78148c 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -19,7 +19,6 @@ #pragma once -#include #include #include @@ -78,8 +77,7 @@ namespace gtsam { /** equality up to tolerance */ bool equals(const Node& q, double tol, - const std::function& - comparator) const override { + const ComparatorFunc& comparator) const override { const Leaf* other = dynamic_cast(&q); if (!other) return false; return comparator(this->constant_, other->constant_, tol); @@ -87,7 +85,7 @@ namespace gtsam { /** print */ void print(const std::string& s, - const std::function& formatter) const override { + const FormatterFunc& formatter) const override { bool showZero = true; if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; } @@ -241,7 +239,7 @@ namespace gtsam { /** print (as a tree) */ void print(const std::string& s, - const std::function& formatter) const override { + const FormatterFunc& formatter) const override { std::cout << s << " Choice("; std::cout << formatter(label_) << ") " << std::endl; for (size_t i = 0; i < branches_.size(); i++) @@ -284,8 +282,7 @@ namespace gtsam { /** equality up to tolerance */ bool equals(const Node& q, double tol, - const std::function& - comparator) const override { + const ComparatorFunc& comparator) const override { const Choice* other = dynamic_cast(&q); if (!other) return false; if (this->label_ != other->label_) return false; @@ -651,16 +648,14 @@ namespace gtsam { /*********************************************************************************/ template - bool DecisionTree::equals( - const DecisionTree& other, double tol, - const std::function& comparator) const { + bool DecisionTree::equals(const DecisionTree& other, double tol, + const ComparatorFunc& comparator) const { return root_->equals(*other.root_, tol, comparator); } template - void DecisionTree::print( - const std::string& s, - const std::function& formatter) const { + void DecisionTree::print(const std::string& s, + const FormatterFunc& formatter) const { root_->print(s, formatter); } diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index eb23bc5ce0..8e6c0e4d73 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -53,6 +53,9 @@ namespace gtsam { public: + using FormatterFunc = std::function; + using ComparatorFunc = std::function; + /** Handy typedefs for unary and binary function types */ typedef std::function Unary; typedef std::function Binary; @@ -91,15 +94,15 @@ namespace gtsam { const void* id() const { return this; } // everything else is virtual, no documentation here as internal - virtual void print(const std::string& s = "", - const std::function& formatter = - &DefaultFormatter) const = 0; + virtual void print( + const std::string& s = "", + const FormatterFunc& formatter = &DefaultFormatter) const = 0; virtual void dot(std::ostream& os, bool showZero) const = 0; virtual bool sameLeaf(const Leaf& q) const = 0; virtual bool sameLeaf(const Node& q) const = 0; - virtual bool equals(const Node& other, double tol = 1e-9, - const std::function& - comparator = &DefaultComparator) const = 0; + virtual bool equals( + const Node& other, double tol = 1e-9, + const ComparatorFunc& comparator = &DefaultComparator) const = 0; virtual const Y& operator()(const Assignment& x) const = 0; virtual Ptr apply(const Unary& op) const = 0; virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; @@ -181,13 +184,11 @@ namespace gtsam { /** GTSAM-style print */ void print(const std::string& s = "DecisionTree", - const std::function& formatter = - &DefaultFormatter) const; + const FormatterFunc& formatter = &DefaultFormatter) const; // Testable bool equals(const DecisionTree& other, double tol = 1e-9, - const std::function& - comparator = &DefaultComparator) const; + const ComparatorFunc& comparator = &DefaultComparator) const; /// @} /// @name Standard Interface From 731cff746bf3d2224883693afe5cbf4d40ccef17 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Dec 2021 17:27:40 -0500 Subject: [PATCH 26/62] rename comparator to compare and capture tol in the function lambda. # Conflicts: # gtsam/hybrid/DCMixtureFactor.h --- gtsam/discrete/AlgebraicDecisionTree.h | 4 ++-- gtsam/discrete/DecisionTree-inl.h | 12 ++++++------ gtsam/discrete/DecisionTree.h | 8 ++++---- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index f47e01668c..acdbf63a39 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -141,10 +141,10 @@ namespace gtsam { /// Equality method customized to node type `double`. bool equals(const AlgebraicDecisionTree& other, double tol = 1e-9) const { // lambda for comparison of two doubles upto some tolerance. - auto comparator = [](double a, double b, double tol) { + auto compare = [tol](double a, double b) { return std::abs(a - b) < tol; }; - return this->root_->equals(*other.root_, tol, comparator); + return this->root_->equals(*other.root_, tol, compare); } }; // AlgebraicDecisionTree diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index fb6c78148c..209c2ad80c 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -77,10 +77,10 @@ namespace gtsam { /** equality up to tolerance */ bool equals(const Node& q, double tol, - const ComparatorFunc& comparator) const override { + const CompareFunc& compare) const override { const Leaf* other = dynamic_cast(&q); if (!other) return false; - return comparator(this->constant_, other->constant_, tol); + return compare(this->constant_, other->constant_); } /** print */ @@ -282,14 +282,14 @@ namespace gtsam { /** equality up to tolerance */ bool equals(const Node& q, double tol, - const ComparatorFunc& comparator) const override { + const CompareFunc& compare) const override { const Choice* other = dynamic_cast(&q); if (!other) return false; if (this->label_ != other->label_) return false; if (branches_.size() != other->branches_.size()) return false; // we don't care about shared pointers being equal here for (size_t i = 0; i < branches_.size(); i++) - if (!(branches_[i]->equals(*(other->branches_[i]), tol, comparator))) + if (!(branches_[i]->equals(*(other->branches_[i]), tol, compare))) return false; return true; } @@ -649,8 +649,8 @@ namespace gtsam { /*********************************************************************************/ template bool DecisionTree::equals(const DecisionTree& other, double tol, - const ComparatorFunc& comparator) const { - return root_->equals(*other.root_, tol, comparator); + const CompareFunc& compare) const { + return root_->equals(*other.root_, tol, compare); } template diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 8e6c0e4d73..26817bf79f 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -47,14 +47,14 @@ namespace gtsam { } /// Default method for comparison of two objects of type Y. - static bool DefaultComparator(const Y& a, const Y& b, double tol) { + static bool DefaultCompare(const Y& a, const Y& b) { return a == b; } public: using FormatterFunc = std::function; - using ComparatorFunc = std::function; + using CompareFunc = std::function; /** Handy typedefs for unary and binary function types */ typedef std::function Unary; @@ -102,7 +102,7 @@ namespace gtsam { virtual bool sameLeaf(const Node& q) const = 0; virtual bool equals( const Node& other, double tol = 1e-9, - const ComparatorFunc& comparator = &DefaultComparator) const = 0; + const CompareFunc& compare = &DefaultCompare) const = 0; virtual const Y& operator()(const Assignment& x) const = 0; virtual Ptr apply(const Unary& op) const = 0; virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; @@ -188,7 +188,7 @@ namespace gtsam { // Testable bool equals(const DecisionTree& other, double tol = 1e-9, - const ComparatorFunc& comparator = &DefaultComparator) const; + const CompareFunc& compare = &DefaultCompare) const; /// @} /// @name Standard Interface From 7f3f332d09acadd5a6dbcacea7a0c3aca24b5a66 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 1 Jan 2022 22:10:54 -0500 Subject: [PATCH 27/62] Removed copy/paste convert --- gtsam/discrete/AlgebraicDecisionTree.h | 9 ++-- gtsam/discrete/DecisionTree-inl.h | 69 ++++++++------------------ gtsam/discrete/DecisionTree.h | 26 ++++------ 3 files changed, 37 insertions(+), 67 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index acdbf63a39..72ea5e79f3 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -108,9 +108,12 @@ namespace gtsam { /** Convert */ template AlgebraicDecisionTree(const AlgebraicDecisionTree& other, - const std::map& map) { - this->root_ = this->template convert(other.root_, map, - Ring::id); + const std::map& map) { + std::function map_function = [&map](const M& label) -> L { + return map.at(label); + }; + std::function op = Ring::id; + this->root_ = this->template convert(other.root_, op, map_function); } /** sum */ diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 209c2ad80c..96f1421ce3 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -453,20 +453,24 @@ namespace gtsam { root_ = compose(functions.begin(), functions.end(), label); } - /*********************************************************************************/ - template - template - DecisionTree::DecisionTree(const DecisionTree& other, - const std::map& map, std::function op) { - root_ = convert(other.root_, map, op); - } - /*********************************************************************************/ template template DecisionTree::DecisionTree(const DecisionTree& other, std::function op) { - root_ = convert(other.root_, op); + auto map = [](const L& label) { return label; }; + root_ = convert(other.root_, op, map); + } + + /*********************************************************************************/ + template + template + DecisionTree::DecisionTree(const DecisionTree& other, + const std::map& map, std::function op) { + std::function map_function = [&map](const M& label) -> L { + return map.at(label); + }; + root_ = convert(other.root_, op, map_function); } /*********************************************************************************/ @@ -579,12 +583,11 @@ namespace gtsam { } /*********************************************************************************/ - template - template + template + template typename DecisionTree::NodePtr DecisionTree::convert( - const typename DecisionTree::NodePtr& f, const std::map& map, - std::function op) { - + const typename DecisionTree::NodePtr& f, + std::function op, std::function map) { typedef DecisionTree MX; typedef typename MX::Leaf MXLeaf; typedef typename MX::Choice MXChoice; @@ -602,50 +605,18 @@ namespace gtsam { "DecisionTree::Convert: Invalid NodePtr"); // get new label - M oldLabel = choice->label(); - L newLabel = map.at(oldLabel); + const M oldLabel = choice->label(); + const L newLabel = map(oldLabel); // put together via Shannon expansion otherwise not sorted. std::vector functions; for(const MXNodePtr& branch: choice->branches()) { - LY converted(convert(branch, map, op)); + LY converted(convert(branch, op, map)); functions += converted; } return LY::compose(functions.begin(), functions.end(), newLabel); } - /*********************************************************************************/ - template - template - typename DecisionTree::NodePtr DecisionTree::convert( - const typename DecisionTree::NodePtr& f, - std::function op) { - - typedef DecisionTree LX; - typedef typename LX::Leaf LXLeaf; - typedef typename LX::Choice LXChoice; - typedef typename LX::NodePtr LXNodePtr; - typedef DecisionTree LY; - - // ugliness below because apparently we can't have templated virtual functions - // If leaf, apply unary conversion "op" and create a unique leaf - const LXLeaf* leaf = dynamic_cast (f.get()); - if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); - - // Check if Choice - boost::shared_ptr choice = boost::dynamic_pointer_cast (f); - if (!choice) throw std::invalid_argument( - "DecisionTree::Convert: Invalid NodePtr"); - - // put together via Shannon expansion otherwise not sorted. - std::vector functions; - for(const LXNodePtr& branch: choice->branches()) { - LY converted(convert(branch, op)); - functions += converted; - } - return LY::compose(functions.begin(), functions.end(), choice->label()); - } - /*********************************************************************************/ template bool DecisionTree::equals(const DecisionTree& other, double tol, diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 26817bf79f..baf2a79fa6 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -127,15 +127,11 @@ namespace gtsam { template NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const; - /** Convert to a different type */ - template NodePtr - convert(const typename DecisionTree::NodePtr& f, const std::map& map, std::function op); - - /** Convert only node to a different type */ - template - NodePtr convert(const typename DecisionTree::NodePtr& f, - const std::function op); + /// Convert to a different type, will not convert label if map empty. + template + NodePtr convert(const typename DecisionTree::NodePtr& f, + std::function op, + std::function map); public: @@ -168,16 +164,16 @@ namespace gtsam { DecisionTree(const L& label, // const DecisionTree& f0, const DecisionTree& f1); - /** Convert from a different type */ - template - DecisionTree(const DecisionTree& other, - const std::map& map, std::function op); - - /** Convert only nodes from a different type */ + /** Convert from a different type. */ template DecisionTree(const DecisionTree& other, std::function op); + /** Convert from a different type, also transate labels via map. */ + template + DecisionTree(const DecisionTree& other, + const std::map& map, std::function op); + /// @} /// @name Testable /// @{ From 78f8cc948d05e1fd11e150e092b638e59dcadf93 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 09:47:15 -0500 Subject: [PATCH 28/62] Define empty and check for it in apply variants --- gtsam/discrete/DecisionTree-inl.h | 10 ++++++++++ gtsam/discrete/DecisionTree.h | 3 +++ gtsam/discrete/tests/testDecisionTree.cpp | 11 +++++++++++ 3 files changed, 24 insertions(+) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 96f1421ce3..fbdeae4602 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -642,6 +642,11 @@ namespace gtsam { template DecisionTree DecisionTree::apply(const Unary& op) const { + // It is unclear what should happen if tree is empty: + if (empty()) { + throw std::runtime_error( + "DecisionTree::apply(unary op) undefined for empty tree."); + } return DecisionTree(root_->apply(op)); } @@ -649,6 +654,11 @@ namespace gtsam { template DecisionTree DecisionTree::apply(const DecisionTree& g, const Binary& op) const { + // It is unclear what should happen if either tree is empty: + if (empty() or g.empty()) { + throw std::runtime_error( + "DecisionTree::apply(binary op) undefined for empty trees."); + } // apply the operaton on the root of both diagrams NodePtr h = root_->apply_f_op_g(*g.root_, op); // create a new class with the resulting root "h" diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index baf2a79fa6..5cf92f157e 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -194,6 +194,9 @@ namespace gtsam { virtual ~DecisionTree() { } + /** empty tree? */ + bool empty() const { return !root_; } + /** equality */ bool operator==(const DecisionTree& q) const; diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index dbf6cc44b5..c7ee6cc2a2 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -78,6 +78,9 @@ struct Ring { static inline int one() { return 1; } + static inline int id(const int& a) { + return a; + } static inline int add(const int& a, const int& b) { return a + b; } @@ -100,6 +103,9 @@ TEST(DT, example) x10[A] = 1, x10[B] = 0; x11[A] = 1, x11[B] = 1; + // empty + DT empty; + // A DT a(A, 0, 5); LONGS_EQUAL(0,a(x00)) @@ -118,6 +124,11 @@ TEST(DT, example) LONGS_EQUAL(5,notb(x10)) DOT(notb); + // Check supplying empty trees yields an exception + CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error); + CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error); + CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error); + // apply, two nodes, in natural order DT anotb = apply(a, notb, &Ring::mul); LONGS_EQUAL(0,anotb(x00)) From db3cb4d9ac53f71872962fbab38eb4d82bf24321 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 13:57:12 -0500 Subject: [PATCH 29/62] Refactor print, equals, convert --- gtsam/discrete/AlgebraicDecisionTree.h | 20 +- gtsam/discrete/DecisionTree-inl.h | 91 ++++---- gtsam/discrete/DecisionTree.h | 54 +++-- gtsam/discrete/tests/testDecisionTree.cpp | 266 +++++++++++++--------- 4 files changed, 253 insertions(+), 178 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 72ea5e79f3..37130ab72b 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -28,7 +28,13 @@ namespace gtsam { * TODO: consider eliminating this class altogether? */ template - class AlgebraicDecisionTree: public DecisionTree { + class GTSAM_EXPORT AlgebraicDecisionTree: public DecisionTree { + /// Default method used by `formatter` when printing. + static std::string DefaultFormatter(const L& x) { + std::stringstream ss; + ss << x; + return ss.str(); + } public: @@ -141,13 +147,23 @@ namespace gtsam { return this->combine(labelC, &Ring::add); } + /// print method customized to node type `double`. + void print(const std::string& s, + const typename Super::LabelFormatter& labelFormatter = + &DefaultFormatter) const { + auto valueFormatter = [](const double& v) { + return (boost::format("%4.2g") % v).str(); + }; + Super::print(s, labelFormatter, valueFormatter); + } + /// Equality method customized to node type `double`. bool equals(const AlgebraicDecisionTree& other, double tol = 1e-9) const { // lambda for comparison of two doubles upto some tolerance. auto compare = [tol](double a, double b) { return std::abs(a - b) < tol; }; - return this->root_->equals(*other.root_, tol, compare); + return Super::equals(other, compare); } }; // AlgebraicDecisionTree diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index fbdeae4602..b317737021 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -76,25 +76,26 @@ namespace gtsam { } /** equality up to tolerance */ - bool equals(const Node& q, double tol, - const CompareFunc& compare) const override { + bool equals(const Node& q, const CompareFunc& compare) const override { const Leaf* other = dynamic_cast(&q); if (!other) return false; return compare(this->constant_, other->constant_); } /** print */ - void print(const std::string& s, - const FormatterFunc& formatter) const override { - bool showZero = true; - if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; + void print(const std::string& s, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter) const override { + std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl; } /** to graphviz file */ - void dot(std::ostream& os, bool showZero) const override { - if (showZero || constant_) os << "\"" << this->id() << "\" [label=\"" - << boost::format("%4.2g") % constant_ - << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, + void dot(std::ostream& os, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, + bool showZero) const override { + std::string value = valueFormatter(constant_); + if (showZero || value.compare("0")) + os << "\"" << this->id() << "\" [label=\"" << value + << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, } /** evaluate */ @@ -238,16 +239,19 @@ namespace gtsam { } /** print (as a tree) */ - void print(const std::string& s, - const FormatterFunc& formatter) const override { + void print(const std::string& s, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter) const override { std::cout << s << " Choice("; - std::cout << formatter(label_) << ") " << std::endl; + std::cout << labelFormatter(label_) << ") " << std::endl; for (size_t i = 0; i < branches_.size(); i++) - branches_[i]->print((boost::format("%s %d") % s % i).str(), formatter); + branches_[i]->print((boost::format("%s %d") % s % i).str(), + labelFormatter, valueFormatter); } /** output to graphviz (as a a graph) */ - void dot(std::ostream& os, bool showZero) const override { + void dot(std::ostream& os, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, + bool showZero) const override { os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_ << "\"]\n"; size_t B = branches_.size(); @@ -257,7 +261,8 @@ namespace gtsam { // Check if zero if (!showZero) { const Leaf* leaf = dynamic_cast (branch.get()); - if (leaf && !leaf->constant()) continue; + std::string value = valueFormatter(leaf->constant()); + if (leaf && value.compare("0")) continue; } os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; @@ -266,7 +271,7 @@ namespace gtsam { if (i > 1) os << " [style=bold]"; } os << std::endl; - branch->dot(os, showZero); + branch->dot(os, labelFormatter, valueFormatter, showZero); } } @@ -280,16 +285,15 @@ namespace gtsam { return (q.isLeaf() && q.sameLeaf(*this)); } - /** equality up to tolerance */ - bool equals(const Node& q, double tol, - const CompareFunc& compare) const override { + /** equality */ + bool equals(const Node& q, const CompareFunc& compare) const override { const Choice* other = dynamic_cast(&q); if (!other) return false; if (this->label_ != other->label_) return false; if (branches_.size() != other->branches_.size()) return false; // we don't care about shared pointers being equal here for (size_t i = 0; i < branches_.size(); i++) - if (!(branches_[i]->equals(*(other->branches_[i]), tol, compare))) + if (!(branches_[i]->equals(*(other->branches_[i]), compare))) return false; return true; } @@ -459,7 +463,7 @@ namespace gtsam { DecisionTree::DecisionTree(const DecisionTree& other, std::function op) { auto map = [](const L& label) { return label; }; - root_ = convert(other.root_, op, map); + root_ = other.template convert(op, map); } /*********************************************************************************/ @@ -470,7 +474,7 @@ namespace gtsam { std::function map_function = [&map](const M& label) -> L { return map.at(label); }; - root_ = convert(other.root_, op, map_function); + root_ = other.template convert(op, map_function); } /*********************************************************************************/ @@ -587,7 +591,7 @@ namespace gtsam { template typename DecisionTree::NodePtr DecisionTree::convert( const typename DecisionTree::NodePtr& f, - std::function op, std::function map) { + std::function op, std::function map) const { typedef DecisionTree MX; typedef typename MX::Leaf MXLeaf; typedef typename MX::Choice MXChoice; @@ -596,11 +600,11 @@ namespace gtsam { // ugliness below because apparently we can't have templated virtual functions // If leaf, apply unary conversion "op" and create a unique leaf - const MXLeaf* leaf = dynamic_cast (f.get()); + auto leaf = boost::dynamic_pointer_cast(f); if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); // Check if Choice - boost::shared_ptr choice = boost::dynamic_pointer_cast (f); + auto choice = boost::dynamic_pointer_cast(f); if (!choice) throw std::invalid_argument( "DecisionTree::Convert: Invalid NodePtr"); @@ -619,15 +623,16 @@ namespace gtsam { /*********************************************************************************/ template - bool DecisionTree::equals(const DecisionTree& other, double tol, + bool DecisionTree::equals(const DecisionTree& other, const CompareFunc& compare) const { - return root_->equals(*other.root_, tol, compare); + return root_->equals(*other.root_, compare); } template void DecisionTree::print(const std::string& s, - const FormatterFunc& formatter) const { - root_->print(s, formatter); + const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter) const { + root_->print(s, labelFormatter, valueFormatter); } template @@ -687,26 +692,34 @@ namespace gtsam { } /*********************************************************************************/ - template - void DecisionTree::dot(std::ostream& os, bool showZero) const { + template + void DecisionTree::dot(std::ostream& os, + const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, + bool showZero) const { os << "digraph G {\n"; - root_->dot(os, showZero); + root_->dot(os, labelFormatter, valueFormatter, showZero); os << " [ordering=out]}" << std::endl; } - template - void DecisionTree::dot(const std::string& name, bool showZero) const { + template + void DecisionTree::dot(const std::string& name, + const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, + bool showZero) const { std::ofstream os((name + ".dot").c_str()); - dot(os, showZero); + dot(os, labelFormatter, valueFormatter, showZero); int result = system( ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed"); } - template - std::string DecisionTree::dot(bool showZero) const { + template + std::string DecisionTree::dot(const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, + bool showZero) const { std::stringstream ss; - dot(ss, showZero); + dot(ss, labelFormatter, valueFormatter, showZero); return ss.str(); } diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 5cf92f157e..4c79c58418 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -39,13 +39,6 @@ namespace gtsam { template class GTSAM_EXPORT DecisionTree { - /// Default method used by `formatter` when printing. - static std::string DefaultFormatter(const L& x) { - std::stringstream ss; - ss << x; - return ss.str(); - } - /// Default method for comparison of two objects of type Y. static bool DefaultCompare(const Y& a, const Y& b) { return a == b; @@ -53,7 +46,8 @@ namespace gtsam { public: - using FormatterFunc = std::function; + using LabelFormatter = std::function; + using ValueFormatter = std::function; using CompareFunc = std::function; /** Handy typedefs for unary and binary function types */ @@ -94,15 +88,16 @@ namespace gtsam { const void* id() const { return this; } // everything else is virtual, no documentation here as internal - virtual void print( - const std::string& s = "", - const FormatterFunc& formatter = &DefaultFormatter) const = 0; - virtual void dot(std::ostream& os, bool showZero) const = 0; + virtual void print(const std::string& s, + const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter) const = 0; + virtual void dot(std::ostream& os, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, + bool showZero) const = 0; virtual bool sameLeaf(const Leaf& q) const = 0; virtual bool sameLeaf(const Node& q) const = 0; - virtual bool equals( - const Node& other, double tol = 1e-9, - const CompareFunc& compare = &DefaultCompare) const = 0; + virtual bool equals(const Node& other, const CompareFunc& compare = + &DefaultCompare) const = 0; virtual const Y& operator()(const Assignment& x) const = 0; virtual Ptr apply(const Unary& op) const = 0; virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; @@ -118,11 +113,11 @@ namespace gtsam { /** A function is a shared pointer to the root of a DT */ typedef typename Node::Ptr NodePtr; + protected: + /* a DecisionTree just contains the root */ NodePtr root_; - protected: - /** Internal recursive function to create from keys, cardinalities, and Y values */ template NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const; @@ -131,7 +126,14 @@ namespace gtsam { template NodePtr convert(const typename DecisionTree::NodePtr& f, std::function op, - std::function map); + std::function map) const; + + /// Convert to a different type, will not convert label if map empty. + template + NodePtr convert(std::function op, + std::function map) const { + return convert(root_, op, map); + } public: @@ -179,11 +181,11 @@ namespace gtsam { /// @{ /** GTSAM-style print */ - void print(const std::string& s = "DecisionTree", - const FormatterFunc& formatter = &DefaultFormatter) const; + void print(const std::string& s, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter) const; // Testable - bool equals(const DecisionTree& other, double tol = 1e-9, + bool equals(const DecisionTree& other, const CompareFunc& compare = &DefaultCompare) const; /// @} @@ -225,13 +227,17 @@ namespace gtsam { } /** output to graphviz format, stream version */ - void dot(std::ostream& os, bool showZero = true) const; + void dot(std::ostream& os, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, bool showZero = true) const; /** output to graphviz format, open a file */ - void dot(const std::string& name, bool showZero = true) const; + void dot(const std::string& name, const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, bool showZero = true) const; /** output to graphviz format string */ - std::string dot(bool showZero = true) const; + std::string dot(const LabelFormatter& labelFormatter, + const ValueFormatter& valueFormatter, + bool showZero = true) const; /// @name Advanced Interface /// @{ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index c7ee6cc2a2..f42a590aea 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -43,34 +43,74 @@ void dot(const T&f, const string& filename) { struct Crazy { int a; double b; +}; - bool equals(const Crazy& other, double tol = 1e-12) const { - return a == other.a && std::abs(b - other.b) < tol; +// bool equals(const Crazy& other, double tol = 1e-12) const { +// return a == other.a && std::abs(b - other.b) < tol; +// } + +// bool operator==(const Crazy& other) const { +// return this->equals(other); +// } +// }; + +struct CrazyDecisionTree : public DecisionTree { + /// print to stdout + void print(const std::string& s = "") const { + auto keyFormatter = [](const std::string& s) { return s; }; + auto valueFormatter = [](const Crazy& v) { + return (boost::format("{%d,%4.2g}") % v.a % v.b).str(); + }; + DecisionTree::print("", keyFormatter, valueFormatter); } - - bool operator==(const Crazy& other) const { - return this->equals(other); + /// Equality method customized to Crazy node type + bool equals(const CrazyDecisionTree& other, double tol = 1e-9) const { + auto compare = [tol](const Crazy& v, const Crazy& w) { + return v.a == w.a && std::abs(v.b - w.b) < tol; + }; + return DecisionTree::equals(other, compare); } }; -typedef DecisionTree CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be) - // traits namespace gtsam { template<> struct traits : public Testable {}; } +GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree) + /* ******************************************************************************** */ // Test string labels and int range /* ******************************************************************************** */ -typedef DecisionTree DT; +struct DT : public DecisionTree { + using DecisionTree::DecisionTree; + DT(const DecisionTree& dt) : root_(dt.root_) {} + + /// print to stdout + void print(const std::string& s = "") const { + auto keyFormatter = [](const std::string& s) { return s; }; + auto valueFormatter = [](const int& v) { + return (boost::format("%d") % v).str(); + }; + DecisionTree::print("", keyFormatter, valueFormatter); + } + // /// Equality method customized to int node type + // bool equals(const CrazyDecisionTree& other, double tol = 1e-9) const { + // auto compare = [tol](const int& v, const int& w) { + // return v.a == w.a && std::abs(v.b - w.b) < tol; + // }; + // return DecisionTree::equals(other, compare); + // } +}; // traits namespace gtsam { template<> struct traits
: public Testable
{}; } +GTSAM_CONCEPT_TESTABLE_INST(DT) + struct Ring { static inline int zero() { return 0; @@ -91,111 +131,111 @@ struct Ring { /* ******************************************************************************** */ // test DT -TEST(DT, example) -{ - // Create labels - string A("A"), B("B"), C("C"); - - // create a value - Assignment x00, x01, x10, x11; - x00[A] = 0, x00[B] = 0; - x01[A] = 0, x01[B] = 1; - x10[A] = 1, x10[B] = 0; - x11[A] = 1, x11[B] = 1; - - // empty - DT empty; - - // A - DT a(A, 0, 5); - LONGS_EQUAL(0,a(x00)) - LONGS_EQUAL(5,a(x10)) - DOT(a); - - // pruned - DT p(A, 2, 2); - LONGS_EQUAL(2,p(x00)) - LONGS_EQUAL(2,p(x10)) - DOT(p); - - // \neg B - DT notb(B, 5, 0); - LONGS_EQUAL(5,notb(x00)) - LONGS_EQUAL(5,notb(x10)) - DOT(notb); - - // Check supplying empty trees yields an exception - CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error); - CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error); - CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error); - - // apply, two nodes, in natural order - DT anotb = apply(a, notb, &Ring::mul); - LONGS_EQUAL(0,anotb(x00)) - LONGS_EQUAL(0,anotb(x01)) - LONGS_EQUAL(25,anotb(x10)) - LONGS_EQUAL(0,anotb(x11)) - DOT(anotb); - - // check pruning - DT pnotb = apply(p, notb, &Ring::mul); - LONGS_EQUAL(10,pnotb(x00)) - LONGS_EQUAL( 0,pnotb(x01)) - LONGS_EQUAL(10,pnotb(x10)) - LONGS_EQUAL( 0,pnotb(x11)) - DOT(pnotb); - - // check pruning - DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); - LONGS_EQUAL(0,zeros(x00)) - LONGS_EQUAL(0,zeros(x01)) - LONGS_EQUAL(0,zeros(x10)) - LONGS_EQUAL(0,zeros(x11)) - DOT(zeros); - - // apply, two nodes, in switched order - DT notba = apply(a, notb, &Ring::mul); - LONGS_EQUAL(0,notba(x00)) - LONGS_EQUAL(0,notba(x01)) - LONGS_EQUAL(25,notba(x10)) - LONGS_EQUAL(0,notba(x11)) - DOT(notba); - - // Test choose 0 - DT actual0 = notba.choose(A, 0); - EXPECT(assert_equal(DT(0.0), actual0)); - DOT(actual0); - - // Test choose 1 - DT actual1 = notba.choose(A, 1); - EXPECT(assert_equal(DT(B, 25, 0), actual1)); - DOT(actual1); - - // apply, two nodes at same level - DT a_and_a = apply(a, a, &Ring::mul); - LONGS_EQUAL(0,a_and_a(x00)) - LONGS_EQUAL(0,a_and_a(x01)) - LONGS_EQUAL(25,a_and_a(x10)) - LONGS_EQUAL(25,a_and_a(x11)) - DOT(a_and_a); - - // create a function on C - DT c(C, 0, 5); - - // and a model assigning stuff to C - Assignment x101; - x101[A] = 1, x101[B] = 0, x101[C] = 1; - - // mul notba with C - DT notbac = apply(notba, c, &Ring::mul); - LONGS_EQUAL(125,notbac(x101)) - DOT(notbac); - - // mul now in different order - DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); - LONGS_EQUAL(125,acnotb(x101)) - DOT(acnotb); -} +// TEST(DT, example) +// { +// // Create labels +// string A("A"), B("B"), C("C"); + +// // create a value +// Assignment x00, x01, x10, x11; +// x00[A] = 0, x00[B] = 0; +// x01[A] = 0, x01[B] = 1; +// x10[A] = 1, x10[B] = 0; +// x11[A] = 1, x11[B] = 1; + +// // empty +// DT empty; + +// // A +// DT a(A, 0, 5); +// LONGS_EQUAL(0,a(x00)) +// LONGS_EQUAL(5,a(x10)) +// DOT(a); + +// // pruned +// DT p(A, 2, 2); +// LONGS_EQUAL(2,p(x00)) +// LONGS_EQUAL(2,p(x10)) +// DOT(p); + +// // \neg B +// DT notb(B, 5, 0); +// LONGS_EQUAL(5,notb(x00)) +// LONGS_EQUAL(5,notb(x10)) +// DOT(notb); + +// // Check supplying empty trees yields an exception +// CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error); +// CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error); +// CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error); + +// // apply, two nodes, in natural order +// DT anotb = apply(a, notb, &Ring::mul); +// LONGS_EQUAL(0,anotb(x00)) +// LONGS_EQUAL(0,anotb(x01)) +// LONGS_EQUAL(25,anotb(x10)) +// LONGS_EQUAL(0,anotb(x11)) +// DOT(anotb); + +// // check pruning +// DT pnotb = apply(p, notb, &Ring::mul); +// LONGS_EQUAL(10,pnotb(x00)) +// LONGS_EQUAL( 0,pnotb(x01)) +// LONGS_EQUAL(10,pnotb(x10)) +// LONGS_EQUAL( 0,pnotb(x11)) +// DOT(pnotb); + +// // check pruning +// DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); +// LONGS_EQUAL(0,zeros(x00)) +// LONGS_EQUAL(0,zeros(x01)) +// LONGS_EQUAL(0,zeros(x10)) +// LONGS_EQUAL(0,zeros(x11)) +// DOT(zeros); + +// // apply, two nodes, in switched order +// DT notba = apply(a, notb, &Ring::mul); +// LONGS_EQUAL(0,notba(x00)) +// LONGS_EQUAL(0,notba(x01)) +// LONGS_EQUAL(25,notba(x10)) +// LONGS_EQUAL(0,notba(x11)) +// DOT(notba); + +// // Test choose 0 +// DT actual0 = notba.choose(A, 0); +// EXPECT(assert_equal(DT(0.0), actual0)); +// DOT(actual0); + +// // Test choose 1 +// DT actual1 = notba.choose(A, 1); +// EXPECT(assert_equal(DT(B, 25, 0), actual1)); +// DOT(actual1); + +// // apply, two nodes at same level +// DT a_and_a = apply(a, a, &Ring::mul); +// LONGS_EQUAL(0,a_and_a(x00)) +// LONGS_EQUAL(0,a_and_a(x01)) +// LONGS_EQUAL(25,a_and_a(x10)) +// LONGS_EQUAL(25,a_and_a(x11)) +// DOT(a_and_a); + +// // create a function on C +// DT c(C, 0, 5); + +// // and a model assigning stuff to C +// Assignment x101; +// x101[A] = 1, x101[B] = 0, x101[C] = 1; + +// // mul notba with C +// DT notbac = apply(notba, c, &Ring::mul); +// LONGS_EQUAL(125,notbac(x101)) +// DOT(notbac); + +// // mul now in different order +// DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); +// LONGS_EQUAL(125,acnotb(x101)) +// DOT(acnotb); +// } /* ******************************************************************************** */ // test Conversion From 5c4038c7c02ed49958fe12336cfef7a9c76bf039 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 18:08:09 -0500 Subject: [PATCH 30/62] Fixed dot to have right arguments --- gtsam/discrete/DecisionTreeFactor.cpp | 25 +++++++++++++++++++++++++ gtsam/discrete/DecisionTreeFactor.h | 14 ++++++++++++++ gtsam/discrete/discrete.i | 4 +++- 3 files changed, 42 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7aed00c57d..75018cf928 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -153,6 +153,31 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + static std::string valueFormatter(const double& v) { + return (boost::format("%4.2g") % v).str(); + } + + /** output to graphviz format, stream version */ + void DecisionTreeFactor::dot(std::ostream& os, + const KeyFormatter& keyFormatter, + bool showZero) const { + Potentials::dot(os, keyFormatter, valueFormatter, showZero); + } + + /** output to graphviz format, open a file */ + void DecisionTreeFactor::dot(const std::string& name, + const KeyFormatter& keyFormatter, + bool showZero) const { + Potentials::dot(name, keyFormatter, valueFormatter, showZero); + } + + /** output to graphviz format string */ + std::string DecisionTreeFactor::dot(const KeyFormatter& keyFormatter, + bool showZero) const { + return Potentials::dot(keyFormatter, valueFormatter, showZero); + } + /* ************************************************************************* */ std::string DecisionTreeFactor::markdown( const KeyFormatter& keyFormatter) const { diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index f90af56dd0..46509db822 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -178,6 +178,20 @@ namespace gtsam { /// @name Wrapper support /// @{ + /** output to graphviz format, stream version */ + void dot(std::ostream& os, + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + bool showZero = true) const; + + /** output to graphviz format, open a file */ + void dot(const std::string& name, + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + bool showZero = true) const; + + /** output to graphviz format string */ + std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + bool showZero = true) const; + /// Render as markdown table. std::string markdown( const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 36caccfc83..5bd4a2913a 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -46,7 +46,9 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const; - string dot(bool showZero = false) const; + string dot( + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, + bool showZero = false) const; std::vector> enumerate() const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From 6c23fd1e866d967e9c752df7d616859963e58df8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 18:08:45 -0500 Subject: [PATCH 31/62] Renamed protected method convert -> convertFrom --- gtsam/discrete/AlgebraicDecisionTree.h | 4 +- gtsam/discrete/DecisionTree-inl.h | 28 +-- gtsam/discrete/DecisionTree.h | 29 +-- gtsam/discrete/tests/testDecisionTree.cpp | 238 +++++++++++----------- 4 files changed, 147 insertions(+), 152 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 37130ab72b..17a38f7cf3 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -115,11 +115,11 @@ namespace gtsam { template AlgebraicDecisionTree(const AlgebraicDecisionTree& other, const std::map& map) { - std::function map_function = [&map](const M& label) -> L { + std::function L_of_M = [&map](const M& label) -> L { return map.at(label); }; std::function op = Ring::id; - this->root_ = this->template convert(other.root_, op, map_function); + this->root_ = this->template convertFrom(other.root_, L_of_M, op); } /** sum */ diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index b317737021..af52a6daf3 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -461,20 +461,21 @@ namespace gtsam { template template DecisionTree::DecisionTree(const DecisionTree& other, - std::function op) { - auto map = [](const L& label) { return label; }; - root_ = other.template convert(op, map); + std::function Y_of_X) { + auto L_of_L = [](const L& label) { return label; }; + root_ = convertFrom(Y_of_X, L_of_L); } /*********************************************************************************/ - template - template + template + template DecisionTree::DecisionTree(const DecisionTree& other, - const std::map& map, std::function op) { - std::function map_function = [&map](const M& label) -> L { + const std::map& map, + std::function Y_of_X) { + std::function L_of_M = [&map](const M& label) -> L { return map.at(label); }; - root_ = other.template convert(op, map_function); + root_ = convertFrom(other.root_, L_of_M, Y_of_X); } /*********************************************************************************/ @@ -589,9 +590,10 @@ namespace gtsam { /*********************************************************************************/ template template - typename DecisionTree::NodePtr DecisionTree::convert( + typename DecisionTree::NodePtr DecisionTree::convertFrom( const typename DecisionTree::NodePtr& f, - std::function op, std::function map) const { + std::function L_of_M, + std::function Y_of_X) const { typedef DecisionTree MX; typedef typename MX::Leaf MXLeaf; typedef typename MX::Choice MXChoice; @@ -601,7 +603,7 @@ namespace gtsam { // ugliness below because apparently we can't have templated virtual functions // If leaf, apply unary conversion "op" and create a unique leaf auto leaf = boost::dynamic_pointer_cast(f); - if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); + if (leaf) return NodePtr(new Leaf(Y_of_X(leaf->constant()))); // Check if Choice auto choice = boost::dynamic_pointer_cast(f); @@ -610,12 +612,12 @@ namespace gtsam { // get new label const M oldLabel = choice->label(); - const L newLabel = map(oldLabel); + const L newLabel = L_of_M(oldLabel); // put together via Shannon expansion otherwise not sorted. std::vector functions; for(const MXNodePtr& branch: choice->branches()) { - LY converted(convert(branch, op, map)); + LY converted(convertFrom(branch, L_of_M, Y_of_X)); functions += converted; } return LY::compose(functions.begin(), functions.end(), newLabel); diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 4c79c58418..ecc3d17dce 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -113,27 +113,20 @@ namespace gtsam { /** A function is a shared pointer to the root of a DT */ typedef typename Node::Ptr NodePtr; - protected: - - /* a DecisionTree just contains the root */ + /// a DecisionTree just contains the root. TODO(dellaert): make protected. NodePtr root_; + protected: + /** Internal recursive function to create from keys, cardinalities, and Y values */ template NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const; - /// Convert to a different type, will not convert label if map empty. + /// Convert from a DecisionTree. template - NodePtr convert(const typename DecisionTree::NodePtr& f, - std::function op, - std::function map) const; - - /// Convert to a different type, will not convert label if map empty. - template - NodePtr convert(std::function op, - std::function map) const { - return convert(root_, op, map); - } + NodePtr convertFrom(const typename DecisionTree::NodePtr& f, + std::function L_of_M, + std::function Y_of_X) const; public: @@ -169,12 +162,12 @@ namespace gtsam { /** Convert from a different type. */ template DecisionTree(const DecisionTree& other, - std::function op); + std::function Y_of_X); /** Convert from a different type, also transate labels via map. */ - template - DecisionTree(const DecisionTree& other, - const std::map& map, std::function op); + template + DecisionTree(const DecisionTree& other, const std::map& L_of_M, + std::function Y_of_X); /// @} /// @name Testable diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index f42a590aea..cc61a382f9 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -84,8 +84,11 @@ GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree) /* ******************************************************************************** */ struct DT : public DecisionTree { + using Base = DecisionTree; using DecisionTree::DecisionTree; - DT(const DecisionTree& dt) : root_(dt.root_) {} + DT() = default; + + DT(const Base& dt) : Base(dt) {} /// print to stdout void print(const std::string& s = "") const { @@ -93,15 +96,13 @@ struct DT : public DecisionTree { auto valueFormatter = [](const int& v) { return (boost::format("%d") % v).str(); }; - DecisionTree::print("", keyFormatter, valueFormatter); + Base::print("", keyFormatter, valueFormatter); + } + /// Equality method customized to int node type + bool equals(const Base& other, double tol = 1e-9) const { + auto compare = [](const int& v, const int& w) { return v == w; }; + return Base::equals(other, compare); } - // /// Equality method customized to int node type - // bool equals(const CrazyDecisionTree& other, double tol = 1e-9) const { - // auto compare = [tol](const int& v, const int& w) { - // return v.a == w.a && std::abs(v.b - w.b) < tol; - // }; - // return DecisionTree::equals(other, compare); - // } }; // traits @@ -131,111 +132,111 @@ struct Ring { /* ******************************************************************************** */ // test DT -// TEST(DT, example) -// { -// // Create labels -// string A("A"), B("B"), C("C"); - -// // create a value -// Assignment x00, x01, x10, x11; -// x00[A] = 0, x00[B] = 0; -// x01[A] = 0, x01[B] = 1; -// x10[A] = 1, x10[B] = 0; -// x11[A] = 1, x11[B] = 1; - -// // empty -// DT empty; - -// // A -// DT a(A, 0, 5); -// LONGS_EQUAL(0,a(x00)) -// LONGS_EQUAL(5,a(x10)) -// DOT(a); - -// // pruned -// DT p(A, 2, 2); -// LONGS_EQUAL(2,p(x00)) -// LONGS_EQUAL(2,p(x10)) -// DOT(p); - -// // \neg B -// DT notb(B, 5, 0); -// LONGS_EQUAL(5,notb(x00)) -// LONGS_EQUAL(5,notb(x10)) -// DOT(notb); - -// // Check supplying empty trees yields an exception -// CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error); -// CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error); -// CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error); - -// // apply, two nodes, in natural order -// DT anotb = apply(a, notb, &Ring::mul); -// LONGS_EQUAL(0,anotb(x00)) -// LONGS_EQUAL(0,anotb(x01)) -// LONGS_EQUAL(25,anotb(x10)) -// LONGS_EQUAL(0,anotb(x11)) -// DOT(anotb); - -// // check pruning -// DT pnotb = apply(p, notb, &Ring::mul); -// LONGS_EQUAL(10,pnotb(x00)) -// LONGS_EQUAL( 0,pnotb(x01)) -// LONGS_EQUAL(10,pnotb(x10)) -// LONGS_EQUAL( 0,pnotb(x11)) -// DOT(pnotb); - -// // check pruning -// DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); -// LONGS_EQUAL(0,zeros(x00)) -// LONGS_EQUAL(0,zeros(x01)) -// LONGS_EQUAL(0,zeros(x10)) -// LONGS_EQUAL(0,zeros(x11)) -// DOT(zeros); - -// // apply, two nodes, in switched order -// DT notba = apply(a, notb, &Ring::mul); -// LONGS_EQUAL(0,notba(x00)) -// LONGS_EQUAL(0,notba(x01)) -// LONGS_EQUAL(25,notba(x10)) -// LONGS_EQUAL(0,notba(x11)) -// DOT(notba); - -// // Test choose 0 -// DT actual0 = notba.choose(A, 0); -// EXPECT(assert_equal(DT(0.0), actual0)); -// DOT(actual0); - -// // Test choose 1 -// DT actual1 = notba.choose(A, 1); -// EXPECT(assert_equal(DT(B, 25, 0), actual1)); -// DOT(actual1); - -// // apply, two nodes at same level -// DT a_and_a = apply(a, a, &Ring::mul); -// LONGS_EQUAL(0,a_and_a(x00)) -// LONGS_EQUAL(0,a_and_a(x01)) -// LONGS_EQUAL(25,a_and_a(x10)) -// LONGS_EQUAL(25,a_and_a(x11)) -// DOT(a_and_a); - -// // create a function on C -// DT c(C, 0, 5); - -// // and a model assigning stuff to C -// Assignment x101; -// x101[A] = 1, x101[B] = 0, x101[C] = 1; - -// // mul notba with C -// DT notbac = apply(notba, c, &Ring::mul); -// LONGS_EQUAL(125,notbac(x101)) -// DOT(notbac); - -// // mul now in different order -// DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); -// LONGS_EQUAL(125,acnotb(x101)) -// DOT(acnotb); -// } +TEST(DT, example) +{ + // Create labels + string A("A"), B("B"), C("C"); + + // create a value + Assignment x00, x01, x10, x11; + x00[A] = 0, x00[B] = 0; + x01[A] = 0, x01[B] = 1; + x10[A] = 1, x10[B] = 0; + x11[A] = 1, x11[B] = 1; + + // empty + DT empty; + + // A + DT a(A, 0, 5); + LONGS_EQUAL(0,a(x00)) + LONGS_EQUAL(5,a(x10)) + DOT(a); + + // pruned + DT p(A, 2, 2); + LONGS_EQUAL(2,p(x00)) + LONGS_EQUAL(2,p(x10)) + DOT(p); + + // \neg B + DT notb(B, 5, 0); + LONGS_EQUAL(5,notb(x00)) + LONGS_EQUAL(5,notb(x10)) + DOT(notb); + + // Check supplying empty trees yields an exception + CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error); + CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error); + CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error); + + // apply, two nodes, in natural order + DT anotb = apply(a, notb, &Ring::mul); + LONGS_EQUAL(0,anotb(x00)) + LONGS_EQUAL(0,anotb(x01)) + LONGS_EQUAL(25,anotb(x10)) + LONGS_EQUAL(0,anotb(x11)) + DOT(anotb); + + // check pruning + DT pnotb = apply(p, notb, &Ring::mul); + LONGS_EQUAL(10,pnotb(x00)) + LONGS_EQUAL( 0,pnotb(x01)) + LONGS_EQUAL(10,pnotb(x10)) + LONGS_EQUAL( 0,pnotb(x11)) + DOT(pnotb); + + // check pruning + DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); + LONGS_EQUAL(0,zeros(x00)) + LONGS_EQUAL(0,zeros(x01)) + LONGS_EQUAL(0,zeros(x10)) + LONGS_EQUAL(0,zeros(x11)) + DOT(zeros); + + // apply, two nodes, in switched order + DT notba = apply(a, notb, &Ring::mul); + LONGS_EQUAL(0,notba(x00)) + LONGS_EQUAL(0,notba(x01)) + LONGS_EQUAL(25,notba(x10)) + LONGS_EQUAL(0,notba(x11)) + DOT(notba); + + // Test choose 0 + DT actual0 = notba.choose(A, 0); + EXPECT(assert_equal(DT(0.0), actual0)); + DOT(actual0); + + // Test choose 1 + DT actual1 = notba.choose(A, 1); + EXPECT(assert_equal(DT(B, 25, 0), actual1)); + DOT(actual1); + + // apply, two nodes at same level + DT a_and_a = apply(a, a, &Ring::mul); + LONGS_EQUAL(0,a_and_a(x00)) + LONGS_EQUAL(0,a_and_a(x01)) + LONGS_EQUAL(25,a_and_a(x10)) + LONGS_EQUAL(25,a_and_a(x11)) + DOT(a_and_a); + + // create a function on C + DT c(C, 0, 5); + + // and a model assigning stuff to C + Assignment x101; + x101[A] = 1, x101[B] = 0, x101[C] = 1; + + // mul notba with C + DT notbac = apply(notba, c, &Ring::mul); + LONGS_EQUAL(125,notbac(x101)) + DOT(notbac); + + // mul now in different order + DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); + LONGS_EQUAL(125,acnotb(x101)) + DOT(acnotb); +} /* ******************************************************************************** */ // test Conversion @@ -243,9 +244,6 @@ enum Label { U, V, X, Y, Z }; typedef DecisionTree BDT; -bool convert(const int& y) { - return y != 0; -} TEST(DT, conversion) { @@ -259,8 +257,10 @@ TEST(DT, conversion) map ordering; ordering[A] = X; ordering[B] = Y; - std::function op = convert; - BDT f2(f1, ordering, op); + std::function bool_of_int = [](const int& y) { + return y != 0; + }; + BDT f2(f1, ordering, bool_of_int); // f1.print("f1"); // f2.print("f2"); From 636425404d18347061114c735b8ffc77cdaf0d69 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 19:57:07 -0500 Subject: [PATCH 32/62] Fix compile error on windows --- gtsam/discrete/DecisionTree-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index af52a6daf3..259489f069 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -662,7 +662,7 @@ namespace gtsam { DecisionTree DecisionTree::apply(const DecisionTree& g, const Binary& op) const { // It is unclear what should happen if either tree is empty: - if (empty() or g.empty()) { + if (empty() || g.empty()) { throw std::runtime_error( "DecisionTree::apply(binary op) undefined for empty trees."); } From 8eb623b58f821c7dbaf7ddda3ac6cc7af54a3f5c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 21:34:22 -0500 Subject: [PATCH 33/62] Added an optional names argument for discrete markdown renderers --- gtsam/discrete/DecisionTreeFactor.cpp | 11 ++++++---- gtsam/discrete/DecisionTreeFactor.h | 12 +++++++--- gtsam/discrete/DiscreteBayesNet.cpp | 5 +++-- gtsam/discrete/DiscreteBayesNet.h | 4 ++-- gtsam/discrete/DiscreteBayesTree.cpp | 5 +++-- gtsam/discrete/DiscreteBayesTree.h | 4 ++-- gtsam/discrete/DiscreteConditional.cpp | 15 ++++++++----- gtsam/discrete/DiscreteConditional.h | 4 ++-- gtsam/discrete/DiscreteFactor.cpp | 15 +++++++++++-- gtsam/discrete/DiscreteFactor.h | 15 +++++++++++-- gtsam/discrete/DiscreteFactorGraph.cpp | 21 ++++++++++-------- gtsam/discrete/DiscreteFactorGraph.h | 15 ++++++++++--- gtsam/discrete/discrete.i | 10 +++++++++ .../discrete/tests/testDecisionTreeFactor.cpp | 21 ++++++++++++++++++ .../tests/testDiscreteConditional.cpp | 22 ++++++++++--------- gtsam_unstable/discrete/Constraint.h | 4 ++-- 16 files changed, 133 insertions(+), 50 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 75018cf928..4f3e3f7f14 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -179,9 +179,9 @@ namespace gtsam { } /* ************************************************************************* */ - std::string DecisionTreeFactor::markdown( - const KeyFormatter& keyFormatter) const { - std::stringstream ss; + string DecisionTreeFactor::markdown(const KeyFormatter& keyFormatter, + const Names& names) const { + stringstream ss; // Print out header and construct argument for `cartesianProduct`. ss << "|"; @@ -200,7 +200,10 @@ namespace gtsam { for (const auto& kv : rows) { ss << "|"; auto assignment = kv.first; - for (auto& key : keys()) ss << assignment.at(key) << "|"; + for (auto& key : keys()) { + size_t index = assignment.at(key); + ss << Translate(names, key, index) << "|"; + } ss << kv.second << "|\n"; } return ss.str(); diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 46509db822..f8832c2237 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -192,9 +192,15 @@ namespace gtsam { std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter, bool showZero = true) const; - /// Render as markdown table. - std::string markdown( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + /** + * @brief Render as markdown table + * + * @param keyFormatter GTSAM-style Key formatter. + * @param names optional, category names corresponding to choices. + * @return std::string a markdown string. + */ + std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const Names& names = {}) const override; /// @} diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index d9fba630e5..510fb56389 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -63,12 +63,13 @@ namespace gtsam { /* ************************************************************************* */ std::string DiscreteBayesNet::markdown( - const KeyFormatter& keyFormatter) const { + const KeyFormatter& keyFormatter, + const DiscreteFactor::Names& names) const { using std::endl; std::stringstream ss; ss << "`DiscreteBayesNet` of size " << size() << endl << endl; for(const DiscreteConditional::shared_ptr& conditional: *this) - ss << conditional->markdown(keyFormatter) << endl; + ss << conditional->markdown(keyFormatter, names) << endl; return ss.str(); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index aed4cec0aa..5332b51dd0 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -108,8 +108,8 @@ namespace gtsam { /// @{ /// Render as markdown table. - std::string markdown( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const DiscreteFactor::Names& names = {}) const; /// @} diff --git a/gtsam/discrete/DiscreteBayesTree.cpp b/gtsam/discrete/DiscreteBayesTree.cpp index 8a9186d05a..07d6e0f0ee 100644 --- a/gtsam/discrete/DiscreteBayesTree.cpp +++ b/gtsam/discrete/DiscreteBayesTree.cpp @@ -57,13 +57,14 @@ namespace gtsam { /* **************************************************************************/ std::string DiscreteBayesTree::markdown( - const KeyFormatter& keyFormatter) const { + const KeyFormatter& keyFormatter, + const DiscreteFactor::Names& names) const { using std::endl; std::stringstream ss; ss << "`DiscreteBayesTree` of size " << nodes_.size() << endl << endl; auto visitor = [&](const DiscreteBayesTreeClique::shared_ptr& clique, size_t& indent) { - ss << "\n" << clique->conditional()->markdown(keyFormatter); + ss << "\n" << clique->conditional()->markdown(keyFormatter, names); return indent + 1; }; size_t indent; diff --git a/gtsam/discrete/DiscreteBayesTree.h b/gtsam/discrete/DiscreteBayesTree.h index 12d6017cc3..6189f25d54 100644 --- a/gtsam/discrete/DiscreteBayesTree.h +++ b/gtsam/discrete/DiscreteBayesTree.h @@ -93,8 +93,8 @@ class GTSAM_EXPORT DiscreteBayesTree /// @{ /// Render as markdown table. - std::string markdown( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const DiscreteFactor::Names& names = {}) const; /// @} }; diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 46d5509e06..203f00f89f 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -283,8 +283,8 @@ size_t DiscreteConditional::sample(size_t parent_value) const { } /* ************************************************************************* */ -std::string DiscreteConditional::markdown( - const KeyFormatter& keyFormatter) const { +std::string DiscreteConditional::markdown(const KeyFormatter& keyFormatter, + const Names& names) const { std::stringstream ss; // Print out signature. @@ -331,7 +331,10 @@ std::string DiscreteConditional::markdown( pairs.rend() - nrParents()); const auto frontal_assignments = cartesianProduct(slatnorf); for (const auto& a : frontal_assignments) { - for (it = beginFrontals(); it != endFrontals(); ++it) ss << a.at(*it); + for (it = beginFrontals(); it != endFrontals(); ++it) { + size_t index = a.at(*it); + ss << Translate(names, *it, index); + } ss << "|"; } ss << "\n"; @@ -348,8 +351,10 @@ std::string DiscreteConditional::markdown( for (const auto& a : assignments) { if (count == 0) { ss << "|"; - for (it = beginParents(); it != endParents(); ++it) - ss << a.at(*it) << "|"; + for (it = beginParents(); it != endParents(); ++it) { + size_t index = a.at(*it); + ss << Translate(names, *it, index) << "|"; + } } ss << operator()(a) << "|"; count = (count + 1) % n; diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index d21e3ae264..1cad927e92 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -180,8 +180,8 @@ class GTSAM_EXPORT DiscreteConditional: public DecisionTreeFactor, /// @{ /// Render as markdown table. - std::string markdown( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const Names& names = {}) const override; /// @} }; diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index c101653d28..1a12ef405a 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -19,9 +19,20 @@ #include +#include + using namespace std; namespace gtsam { -/* ************************************************************************* */ -} // namespace gtsam +string DiscreteFactor::Translate(const Names& names, Key key, size_t index) { + if (names.empty()) { + stringstream ss; + ss << index; + return ss.str(); + } else { + return names.at(key)[index]; + } +} + +} // namespace gtsam diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 76ed703bb2..70545a5ca5 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -89,9 +89,20 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { /// @name Wrapper support /// @{ - /// Render as markdown table. + using Names = std::map>; + + static std::string Translate(const Names& names, Key key, size_t index); + + /** + * @brief Render as markdown table + * + * @param keyFormatter GTSAM-style Key formatter. + * @param names optional, category names corresponding to choices. + * @return std::string a markdown string. + */ virtual std::string markdown( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const Names& names = {}) const = 0; /// @} }; diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index bd84e13647..be046d2902 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -16,15 +16,17 @@ * @author Frank Dellaert */ -//#define ENABLE_TIMING -#include -#include #include +#include #include +#include #include -#include #include -#include +#include + +using std::vector; +using std::string; +using std::map; namespace gtsam { @@ -64,7 +66,7 @@ namespace gtsam { } /* ************************************************************************* */ - void DiscreteFactorGraph::print(const std::string& s, + void DiscreteFactorGraph::print(const string& s, const KeyFormatter& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; @@ -130,14 +132,15 @@ namespace gtsam { } /* ************************************************************************* */ - std::string DiscreteFactorGraph::markdown( - const KeyFormatter& keyFormatter) const { + string DiscreteFactorGraph::markdown( + const KeyFormatter& keyFormatter, + const DiscreteFactor::Names& names) const { using std::endl; std::stringstream ss; ss << "`DiscreteFactorGraph` of size " << size() << endl << endl; for (size_t i = 0; i < factors_.size(); i++) { ss << "factor " << i << ":\n"; - ss << factors_[i]->markdown(keyFormatter) << endl; + ss << factors_[i]->markdown(keyFormatter, names) << endl; } return ss.str(); } diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 6856493f7f..9aa04d6497 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -24,7 +24,10 @@ #include #include #include + #include +#include +#include namespace gtsam { @@ -140,9 +143,15 @@ public EliminateableFactorGraph { /// @name Wrapper support /// @{ - /// Render as markdown table. - std::string markdown( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** + * @brief Render as markdown table + * + * @param keyFormatter GTSAM-style Key formatter. + * @param names optional, a map from Key to category names. + * @return std::string a (potentially long) markdown string. + */ + std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const DiscreteFactor::Names& names = {}) const; /// @} }; // \ DiscreteFactorGraph diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 5bd4a2913a..e298deaf1b 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -52,6 +52,8 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { std::vector> enumerate() const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + string markdown(const gtsam::KeyFormatter& keyFormatter, + std::map> names) const; }; #include @@ -88,6 +90,8 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor { void sampleInPlace(gtsam::DiscreteValues @parentsValues) const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + string markdown(const gtsam::KeyFormatter& keyFormatter, + std::map> names) const; }; #include @@ -130,6 +134,8 @@ class DiscreteBayesNet { gtsam::DiscreteValues sample() const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + string markdown(const gtsam::KeyFormatter& keyFormatter, + std::map> names) const; }; #include @@ -164,6 +170,8 @@ class DiscreteBayesTree { string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + string markdown(const gtsam::KeyFormatter& keyFormatter, + std::map> names) const; }; #include @@ -211,6 +219,8 @@ class DiscreteFactorGraph { string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + string markdown(const gtsam::KeyFormatter& keyFormatter, + std::map> names) const; }; } // namespace gtsam diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 6af7ca7313..c4e5f06bb3 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -119,6 +119,27 @@ TEST(DecisionTreeFactor, markdown) { EXPECT(actual == expected); } +/* ************************************************************************* */ +// Check markdown representation with a value formatter. +TEST(DecisionTreeFactor, markdownWithValueFormatter) { + DiscreteKey A(12, 3), B(5, 2); + DecisionTreeFactor f(A & B, "1 2 3 4 5 6"); + string expected = + "|A|B|value|\n" + "|:-:|:-:|:-:|\n" + "|Zero|-|1|\n" + "|Zero|+|2|\n" + "|One|-|3|\n" + "|One|+|4|\n" + "|Two|-|5|\n" + "|Two|+|6|\n"; + auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; }; + DecisionTreeFactor::Names names{{12, {"Zero", "One", "Two"}}, + {5, {"-", "+"}}}; + string actual = f.markdown(keyFormatter, names); + EXPECT(actual == expected); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 00ae1acd01..90da07cdcd 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -161,17 +161,19 @@ TEST(DiscreteConditional, markdown) { DiscreteConditional conditional(A, {B, C}, "0/1 1/3 1/1 3/1 0/1 1/0"); string expected = " *P(A|B,C)*:\n\n" - "|B|C|0|1|\n" + "|B|C|T|F|\n" "|:-:|:-:|:-:|:-:|\n" - "|0|0|0|1|\n" - "|0|1|0.25|0.75|\n" - "|0|2|0.5|0.5|\n" - "|1|0|0.75|0.25|\n" - "|1|1|0|1|\n" - "|1|2|1|0|\n"; - vector names{"C", "B", "A"}; - auto formatter = [names](Key key) { return names[key]; }; - string actual = conditional.markdown(formatter); + "|-|Zero|0|1|\n" + "|-|One|0.25|0.75|\n" + "|-|Two|0.5|0.5|\n" + "|+|Zero|0.75|0.25|\n" + "|+|One|0|1|\n" + "|+|Two|1|0|\n"; + vector keyNames{"C", "B", "A"}; + auto formatter = [keyNames](Key key) { return keyNames[key]; }; + DecisionTreeFactor::Names names{ + {0, {"Zero", "One", "Two"}}, {1, {"-", "+"}}, {2, {"T", "F"}}}; + string actual = conditional.markdown(formatter, names); EXPECT(actual == expected); } diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 5c21028a0c..85748f0546 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -86,8 +86,8 @@ class GTSAM_EXPORT Constraint : public DiscreteFactor { /// @{ /// Render as markdown table. - std::string markdown( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const Names& names = {}) const override { return (boost::format("`Constraint` on %1% variables\n") % (size())).str(); } From c51bba81d8cf8ebf81e67bed6a33be3dd2e681e3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 28 Dec 2021 21:22:03 -0500 Subject: [PATCH 34/62] Fix sample() --- gtsam/discrete/DiscretePrior.h | 2 +- python/gtsam/tests/test_DiscretePrior.py | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/DiscretePrior.h b/gtsam/discrete/DiscretePrior.h index 1a7c6ae6cb..d11d9be066 100644 --- a/gtsam/discrete/DiscretePrior.h +++ b/gtsam/discrete/DiscretePrior.h @@ -98,7 +98,7 @@ class GTSAM_EXPORT DiscretePrior : public DiscreteConditional { * sample * @return sample from conditional */ - size_t sample() const { return Base::sample({}); } + size_t sample() const { return Base::sample(DiscreteValues()); } /// @} }; diff --git a/python/gtsam/tests/test_DiscretePrior.py b/python/gtsam/tests/test_DiscretePrior.py index 4f017d66a4..5bf6a8d196 100644 --- a/python/gtsam/tests/test_DiscretePrior.py +++ b/python/gtsam/tests/test_DiscretePrior.py @@ -6,7 +6,7 @@ See LICENSE for the license information Unit tests for Discrete Priors. -Author: Varun Agrawal +Author: Frank Dellaert """ # pylint: disable=no-name-in-module, invalid-name @@ -42,6 +42,11 @@ def test_pmf(self): expected = np.array([0.4, 0.6]) np.testing.assert_allclose(expected, prior.pmf()) + def test_sample(self): + prior = DiscretePrior(X, "2/3") + actual = prior.sample() + self.assertIsInstance(actual, int) + def test_markdown(self): """Test the _repr_markdown_ method.""" From fca23e0559a39e3a1c402c0c0ebe18bebb5b71a2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 22:38:39 -0500 Subject: [PATCH 35/62] italicized parent values --- gtsam/discrete/DiscreteConditional.cpp | 2 +- gtsam/discrete/tests/testDiscreteBayesNet.cpp | 3 ++- gtsam/discrete/tests/testDiscreteConditional.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 203f00f89f..af4ad4495d 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -317,7 +317,7 @@ std::string DiscreteConditional::markdown(const KeyFormatter& keyFormatter, ss << "|"; const_iterator it; for(Key parent: parents()) { - ss << keyFormatter(parent) << "|"; + ss << "*" << keyFormatter(parent) << "*|"; pairs.emplace_back(parent, cardinalities_.at(parent)); } diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 1de45905a6..de8e05edc3 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -187,12 +187,13 @@ TEST(DiscreteBayesNet, markdown) { "|1|0.01|\n" "\n" " *P(Smoking|Asia)*:\n\n" - "|Asia|0|1|\n" + "|*Asia*|0|1|\n" "|:-:|:-:|:-:|\n" "|0|0.8|0.2|\n" "|1|0.7|0.3|\n\n"; auto formatter = [](Key key) { return key == 0 ? "Asia" : "Smoking"; }; string actual = fragment.markdown(formatter); + cout << actual << endl; EXPECT(actual == expected); } diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 90da07cdcd..b498b0541a 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -143,7 +143,7 @@ TEST(DiscreteConditional, markdown_multivalued) { A | B = "2/88/10 2/20/78 33/33/34 33/33/34 95/2/3"); string expected = " *P(a1|b1)*:\n\n" - "|b1|0|1|2|\n" + "|*b1*|0|1|2|\n" "|:-:|:-:|:-:|:-:|\n" "|0|0.02|0.88|0.1|\n" "|1|0.02|0.2|0.78|\n" @@ -161,7 +161,7 @@ TEST(DiscreteConditional, markdown) { DiscreteConditional conditional(A, {B, C}, "0/1 1/3 1/1 3/1 0/1 1/0"); string expected = " *P(A|B,C)*:\n\n" - "|B|C|T|F|\n" + "|*B*|*C*|T|F|\n" "|:-:|:-:|:-:|:-:|\n" "|-|Zero|0|1|\n" "|-|One|0.25|0.75|\n" From 88c79a2a56f564a64b30e520f5074b8b283c3111 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 22:48:55 -0500 Subject: [PATCH 36/62] Fixed some examples --- gtsam_unstable/discrete/examples/schedulingExample.cpp | 2 +- gtsam_unstable/discrete/examples/schedulingQuals12.cpp | 2 +- gtsam_unstable/discrete/examples/schedulingQuals13.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index 3460664db7..2a9addf918 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -115,7 +115,7 @@ void runLargeExample() { // Do brute force product and output that to file if (scheduler.nrStudents() == 1) { // otherwise too slow DecisionTreeFactor product = scheduler.product(); - product.dot("scheduling-large", false); + product.dot("scheduling-large", DefaultKeyFormatter, false); } // Do exact inference diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 19694c31ec..8260bfb068 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -115,7 +115,7 @@ void runLargeExample() { // Do brute force product and output that to file if (scheduler.nrStudents() == 1) { // otherwise too slow DecisionTreeFactor product = scheduler.product(); - product.dot("scheduling-large", false); + product.dot("scheduling-large", DefaultKeyFormatter, false); } // Do exact inference diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 4b96b1eeba..cf3ce04535 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -139,7 +139,7 @@ void runLargeExample() { // Do brute force product and output that to file if (scheduler.nrStudents() == 1) { // otherwise too slow DecisionTreeFactor product = scheduler.product(); - product.dot("scheduling-large", false); + product.dot("scheduling-large", DefaultKeyFormatter, false); } // Do exact inference From 53a6523943392afba36f6f679e501cdc607b459a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 23:23:51 -0500 Subject: [PATCH 37/62] Fixed issues with sample --- gtsam/discrete/DiscreteConditional.cpp | 9 +++++++++ gtsam/discrete/DiscreteConditional.h | 5 ++++- gtsam/discrete/DiscretePrior.h | 2 +- gtsam/discrete/discrete.i | 2 +- gtsam/discrete/tests/testDiscretePrior.cpp | 10 +++++++++- 5 files changed, 24 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index af4ad4495d..b4f95780d5 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -282,6 +282,15 @@ size_t DiscreteConditional::sample(size_t parent_value) const { return sample(values); } +/* ******************************************************************************** */ +size_t DiscreteConditional::sample() const { + if (nrParents() != 0) + throw std::invalid_argument( + "sample() can only be invoked on no-parent prior"); + DiscreteValues values; + return sample(values); +} + /* ************************************************************************* */ std::string DiscreteConditional::markdown(const KeyFormatter& keyFormatter, const Names& names) const { diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 1cad927e92..7ce3dc9308 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -162,9 +162,12 @@ class GTSAM_EXPORT DiscreteConditional: public DecisionTreeFactor, size_t sample(const DiscreteValues& parentsValues) const; - /// Single value version. + /// Single parent version. size_t sample(size_t parent_value) const; + /// Zero parent version. + size_t sample() const; + /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/discrete/DiscretePrior.h b/gtsam/discrete/DiscretePrior.h index d11d9be066..9ac8acb17a 100644 --- a/gtsam/discrete/DiscretePrior.h +++ b/gtsam/discrete/DiscretePrior.h @@ -98,7 +98,7 @@ class GTSAM_EXPORT DiscretePrior : public DiscreteConditional { * sample * @return sample from conditional */ - size_t sample() const { return Base::sample(DiscreteValues()); } + size_t sample() const { return Base::sample(); } /// @} }; diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index e298deaf1b..a837328838 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -86,6 +86,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor { size_t solve(const gtsam::DiscreteValues& parentsValues) const; size_t sample(const gtsam::DiscreteValues& parentsValues) const; size_t sample(size_t value) const; + size_t sample() const; void solveInPlace(gtsam::DiscreteValues @parentsValues) const; void sampleInPlace(gtsam::DiscreteValues @parentsValues) const; string markdown(const gtsam::KeyFormatter& keyFormatter = @@ -105,7 +106,6 @@ virtual class DiscretePrior : gtsam::DiscreteConditional { double operator()(size_t value) const; std::vector pmf() const; size_t solve() const; - size_t sample() const; }; #include diff --git a/gtsam/discrete/tests/testDiscretePrior.cpp b/gtsam/discrete/tests/testDiscretePrior.cpp index b91926cc05..23f093b229 100644 --- a/gtsam/discrete/tests/testDiscretePrior.cpp +++ b/gtsam/discrete/tests/testDiscretePrior.cpp @@ -28,6 +28,8 @@ static const DiscreteKey X(0, 2); /* ************************************************************************* */ TEST(DiscretePrior, constructors) { DiscretePrior actual(X % "2/3"); + EXPECT_LONGS_EQUAL(1, actual.nrFrontals()); + EXPECT_LONGS_EQUAL(0, actual.nrParents()); DecisionTreeFactor f(X, "0.4 0.6"); DiscretePrior expected(f); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -41,12 +43,18 @@ TEST(DiscretePrior, operator) { } /* ************************************************************************* */ -TEST(DiscretePrior, to_vector) { +TEST(DiscretePrior, pmf) { DiscretePrior prior(X % "2/3"); vector expected {0.4, 0.6}; EXPECT(prior.pmf() == expected); } +/* ************************************************************************* */ +TEST(DiscretePrior, sample) { + DiscretePrior prior(X % "2/3"); + prior.sample(); +} + /* ************************************************************************* */ int main() { TestResult tr; From 8c3d51262996d8235ebc1e8e168e45ff916f7c57 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 23:24:03 -0500 Subject: [PATCH 38/62] Fixed python test --- python/gtsam/tests/test_DiscreteConditional.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_DiscreteConditional.py b/python/gtsam/tests/test_DiscreteConditional.py index 1b2ce70cd7..86bc303a9a 100644 --- a/python/gtsam/tests/test_DiscreteConditional.py +++ b/python/gtsam/tests/test_DiscreteConditional.py @@ -50,7 +50,7 @@ def test_markdown(self): "0/1 1/3 1/1 3/1 0/1 1/0") expected = \ " *P(A|B,C)*:\n\n" \ - "|B|C|0|1|\n" \ + "|*B*|*C*|0|1|\n" \ "|:-:|:-:|:-:|:-:|\n" \ "|0|0|0|1|\n" \ "|0|1|0.25|0.75|\n" \ From a9b2c326693b6c087b190628a3fd3780c671a094 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 2 Jan 2022 23:45:01 -0500 Subject: [PATCH 39/62] Move DefaultFormatter to base class and add defaults. Also replace Super with Base and add using. --- gtsam/discrete/AlgebraicDecisionTree.h | 48 +++++++++--------- gtsam/discrete/DecisionTree.h | 69 +++++++++++++++++++------- 2 files changed, 75 insertions(+), 42 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 17a38f7cf3..0b13f408e4 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -29,16 +29,9 @@ namespace gtsam { */ template class GTSAM_EXPORT AlgebraicDecisionTree: public DecisionTree { - /// Default method used by `formatter` when printing. - static std::string DefaultFormatter(const L& x) { - std::stringstream ss; - ss << x; - return ss.str(); - } - public: - typedef DecisionTree Super; + using Base = DecisionTree; /** The Real ring with addition and multiplication */ struct Ring { @@ -66,33 +59,33 @@ namespace gtsam { }; AlgebraicDecisionTree() : - Super(1.0) { + Base(1.0) { } - AlgebraicDecisionTree(const Super& add) : - Super(add) { + AlgebraicDecisionTree(const Base& add) : + Base(add) { } /** Create a new leaf function splitting on a variable */ AlgebraicDecisionTree(const L& label, double y1, double y2) : - Super(label, y1, y2) { + Base(label, y1, y2) { } /** Create a new leaf function splitting on a variable */ - AlgebraicDecisionTree(const typename Super::LabelC& labelC, double y1, double y2) : - Super(labelC, y1, y2) { + AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1, double y2) : + Base(labelC, y1, y2) { } /** Create from keys and vector table */ AlgebraicDecisionTree // - (const std::vector& labelCs, const std::vector& ys) { - this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), + (const std::vector& labelCs, const std::vector& ys) { + this->root_ = Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); } /** Create from keys and string table */ AlgebraicDecisionTree // - (const std::vector& labelCs, const std::string& table) { + (const std::vector& labelCs, const std::string& table) { // Convert string to doubles std::vector ys; std::istringstream iss(table); @@ -100,18 +93,23 @@ namespace gtsam { std::istream_iterator(), std::back_inserter(ys)); // now call recursive Create - this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), + this->root_ = Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); } /** Create a new function splitting on a variable */ template AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : - Super(nullptr) { + Base(nullptr) { this->root_ = compose(begin, end, label); } - /** Convert */ + /** + * Convert labels from type M to type L. + * + * @param other: The AlgebraicDecisionTree with label type M to convert. + * @param map: Map from label type M to label type L. + */ template AlgebraicDecisionTree(const AlgebraicDecisionTree& other, const std::map& map) { @@ -143,18 +141,18 @@ namespace gtsam { } /** sum out variable */ - AlgebraicDecisionTree sum(const typename Super::LabelC& labelC) const { + AlgebraicDecisionTree sum(const typename Base::LabelC& labelC) const { return this->combine(labelC, &Ring::add); } /// print method customized to node type `double`. void print(const std::string& s, - const typename Super::LabelFormatter& labelFormatter = - &DefaultFormatter) const { + const typename Base::LabelFormatter& labelFormatter = + &Base::DefaultFormatter) const { auto valueFormatter = [](const double& v) { return (boost::format("%4.2g") % v).str(); }; - Super::print(s, labelFormatter, valueFormatter); + Base::print(s, labelFormatter, valueFormatter); } /// Equality method customized to node type `double`. @@ -163,7 +161,7 @@ namespace gtsam { auto compare = [tol](double a, double b) { return std::abs(a - b) < tol; }; - return Super::equals(other, compare); + return Base::equals(other, compare); } }; // AlgebraicDecisionTree diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index ecc3d17dce..b02c2b3023 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -39,11 +39,24 @@ namespace gtsam { template class GTSAM_EXPORT DecisionTree { + protected: /// Default method for comparison of two objects of type Y. static bool DefaultCompare(const Y& a, const Y& b) { return a == b; } + /** + * @brief Default method used by `labelFormatter` or `valueFormatter` when printing. + * + * @param x The value passed to format. + * @return std::string + */ + static std::string DefaultFormatter(const L& x) { + std::stringstream ss; + ss << x; + return ss.str(); + } + public: using LabelFormatter = std::function; @@ -88,12 +101,14 @@ namespace gtsam { const void* id() const { return this; } // everything else is virtual, no documentation here as internal - virtual void print(const std::string& s, - const LabelFormatter& labelFormatter, - const ValueFormatter& valueFormatter) const = 0; - virtual void dot(std::ostream& os, const LabelFormatter& labelFormatter, - const ValueFormatter& valueFormatter, - bool showZero) const = 0; + virtual void print( + const std::string& s, + const LabelFormatter& labelFormatter = &DefaultFormatter, + const ValueFormatter& valueFormatter = &DefaultFormatter) const = 0; + virtual void dot(std::ostream& os, + const LabelFormatter& labelFormatter = &DefaultFormatter, + const ValueFormatter& valueFormatter = &DefaultFormatter, + bool showZero = true) const = 0; virtual bool sameLeaf(const Leaf& q) const = 0; virtual bool sameLeaf(const Node& q) const = 0; virtual bool equals(const Node& other, const CompareFunc& compare = @@ -111,7 +126,7 @@ namespace gtsam { public: /** A function is a shared pointer to the root of a DT */ - typedef typename Node::Ptr NodePtr; + using NodePtr = typename Node::Ptr; /// a DecisionTree just contains the root. TODO(dellaert): make protected. NodePtr root_; @@ -164,7 +179,16 @@ namespace gtsam { DecisionTree(const DecisionTree& other, std::function Y_of_X); - /** Convert from a different type, also transate labels via map. */ + /** + * @brief Convert from a different node type X to node type Y, also transate + * labels via map from type M to L. + * + * @tparam M Previous label type. + * @tparam X Previous node type. + * @param other The decision tree to convert. + * @param L_of_M Map from label type M to type L. + * @param Y_of_X Functor to convert from type X to type Y. + */ template DecisionTree(const DecisionTree& other, const std::map& L_of_M, std::function Y_of_X); @@ -173,9 +197,16 @@ namespace gtsam { /// @name Testable /// @{ - /** GTSAM-style print */ - void print(const std::string& s, const LabelFormatter& labelFormatter, - const ValueFormatter& valueFormatter) const; + /** + * @brief GTSAM-style print + * + * @param s Prefix string. + * @param labelFormatter Functor to format the node label. + * @param valueFormatter Functor to format the node value. + */ + void print(const std::string& s, + const LabelFormatter& labelFormatter = &DefaultFormatter, + const ValueFormatter& valueFormatter = &DefaultFormatter) const; // Testable bool equals(const DecisionTree& other, @@ -220,16 +251,20 @@ namespace gtsam { } /** output to graphviz format, stream version */ - void dot(std::ostream& os, const LabelFormatter& labelFormatter, - const ValueFormatter& valueFormatter, bool showZero = true) const; + void dot(std::ostream& os, + const LabelFormatter& labelFormatter = &DefaultFormatter, + const ValueFormatter& valueFormatter = &DefaultFormatter, + bool showZero = true) const; /** output to graphviz format, open a file */ - void dot(const std::string& name, const LabelFormatter& labelFormatter, - const ValueFormatter& valueFormatter, bool showZero = true) const; + void dot(const std::string& name, + const LabelFormatter& labelFormatter = &DefaultFormatter, + const ValueFormatter& valueFormatter = &DefaultFormatter, + bool showZero = true) const; /** output to graphviz format string */ - std::string dot(const LabelFormatter& labelFormatter, - const ValueFormatter& valueFormatter, + std::string dot(const LabelFormatter& labelFormatter = &DefaultFormatter, + const ValueFormatter& valueFormatter = &DefaultFormatter, bool showZero = true) const; /// @name Advanced Interface From 174490eb510dc39b5cc2b9f2c50764081f99f092 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 2 Jan 2022 23:49:47 -0500 Subject: [PATCH 40/62] kill commented out code --- gtsam/discrete/tests/testDecisionTree.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index cc61a382f9..53f3c43797 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -45,15 +45,6 @@ struct Crazy { double b; }; -// bool equals(const Crazy& other, double tol = 1e-12) const { -// return a == other.a && std::abs(b - other.b) < tol; -// } - -// bool operator==(const Crazy& other) const { -// return this->equals(other); -// } -// }; - struct CrazyDecisionTree : public DecisionTree { /// print to stdout void print(const std::string& s = "") const { @@ -261,8 +252,6 @@ TEST(DT, conversion) return y != 0; }; BDT f2(f1, ordering, bool_of_int); - // f1.print("f1"); - // f2.print("f2"); // create a value Assignment