12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use noiseModel for named constructors

release/4.3a0
Frank Dellaert 2022-02-06 13:57:52 -05:00
parent 476a2539f4
commit bc233fc967
3 changed files with 33 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/linear/GaussianConditional.cpp
View File

@ -63,10 +63,11 @@ namespace gtsam {
GaussianConditional GaussianConditional::FromMeanAndStddev( GaussianConditional GaussianConditional::FromMeanAndStddev(
Key key, const Matrix& A, Key parent, const Vector& b, double sigma) { Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
// |Rx + Sy - d| = |x-(Ay + b)|/sigma // |Rx + Sy - d| = |x-(Ay + b)|/sigma
const Matrix R = Matrix::Identity(b.size(), b.size()) / sigma; const Matrix R = Matrix::Identity(b.size(), b.size());
const Matrix S = -A / sigma; const Matrix S = -A;
const Vector d = b / sigma; const Vector d = b;
return GaussianConditional(key, d, R, parent, S); return GaussianConditional(key, d, R, parent, S,
noiseModel::Isotropic::Sigma(b.size(), sigma));
} }
/* ************************************************************************ */ /* ************************************************************************ */
@ -74,11 +75,12 @@ namespace gtsam {
Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2, Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2,
const Vector& b, double sigma) { const Vector& b, double sigma) {
// |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma
const Matrix R = Matrix::Identity(b.size(), b.size()) / sigma; const Matrix R = Matrix::Identity(b.size(), b.size());
const Matrix S = -A1 / sigma; const Matrix S = -A1;
const Matrix T = -A2 / sigma; const Matrix T = -A2;
const Vector d = b / sigma; const Vector d = b;
return GaussianConditional(key, d, R, parent1, S, parent2, T); return GaussianConditional(key, d, R, parent1, S, parent2, T,
noiseModel::Isotropic::Sigma(b.size(), sigma));
} }
/* ************************************************************************ */ /* ************************************************************************ */

5
gtsam/linear/GaussianDensity.cpp
View File

@ -26,8 +26,9 @@ namespace gtsam {
GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, GaussianDensity GaussianDensity::FromMeanAndStddev(Key key,
const Vector& mean, const Vector& mean,
double sigma) { double sigma) {
return GaussianDensity(key, mean / sigma, return GaussianDensity(key, mean,
Matrix::Identity(mean.size(), mean.size()) / sigma); Matrix::Identity(mean.size(), mean.size()),
noiseModel::Isotropic::Sigma(mean.size(), sigma));
} }
/* ************************************************************************* */ /* ************************************************************************* */

19
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -17,10 +17,13 @@
**/ **/
#include <gtsam/linear/GaussianDensity.h> #include <gtsam/linear/GaussianDensity.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
using symbol_shorthand::X;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GaussianDensity, constructor) TEST(GaussianDensity, constructor)
@ -37,6 +40,22 @@ TEST(GaussianDensity, constructor)
EXPECT(assert_equal(s, copied.get_model()->sigmas())); EXPECT(assert_equal(s, copied.get_model()->sigmas()));
} }
/* ************************************************************************* */
// Test FromMeanAndStddev named constructor
TEST(GaussianDensity, FromMeanAndStddev) {
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
const Vector2 b(20, 40), x0(1, 2);
const double sigma = 3;
VectorValues values;
values.insert(X(0), x0);
auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
Vector2 e = (x0 - b) / sigma;
double expected = 0.5 * e.dot(e);
EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9);
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */