diff --git a/gtsam.h b/gtsam.h index 1094d9dd9..8ee778f4c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1458,7 +1458,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1469,7 +1469,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1480,7 +1480,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1491,7 +1491,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1502,7 +1502,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1513,7 +1513,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1524,7 +1524,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1535,7 +1535,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1546,7 +1546,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; }///\namespace mEstimator diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8433bbb01..ec0450abb 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1008,7 +1008,14 @@ TEST(Pose3, print) { // Generate the expected output std::stringstream expected; Point3 translation(1, 2, 3); + +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + expected << "1\n" + "2\n" + "3;\n"; +#else expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';"; +#endif // reset cout to the original stream std::cout.rdbuf(oldbuf); diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 8d569a5df..d1c3adb35 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -36,12 +36,12 @@ namespace noiseModel { * The mEstimator name space contains all robust error functions. * It mirrors the exposition at * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice. + * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: * * Name Symbol Least-Squares L1-norm Huber - * Loss \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; @@ -134,7 +135,7 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} double weight(double /*error*/) const { return 1.0; } - double loss(double error) const { return 0.5 * error * error; } + double residual(double error) const { return error; } void print(const std::string &s) const; bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create(); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index e879731cb..d6b133b98 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair) DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionHuber) @@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber) DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); - DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); - DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); - DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); - DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); + DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); + DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); + DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); + DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionCauchy) @@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) @@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionWelsch) @@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch) DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionTukey) @@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); - DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); - DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); + DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8); + DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8); } TEST(NoiseModel, robustFunctionL2WithDeadZone) @@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone) DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); } /* ************************************************************************* */ diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index ce505df5d..8a0485334 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename) rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); - rho(i) = model.loss(x(i)); + rho(i) = model.residual(x(i)); end psi = w .* x;