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

Remove obsolete test

release/4.3a0
Frank Dellaert 2023-01-03 15:21:44 -05:00
parent 4f2615aeeb
commit 65aaf47a1c
1 changed files with 0 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
tests/testNonlinearFactorGraph.cpp
View File

@ -389,83 +389,6 @@ TEST(NonlinearFactorGraph, dot_extra) {
EXPECT(ss.str() == expected); EXPECT(ss.str() == expected);
} }
/* ************************************************************************* */
template <class VALUE>
class MyPrior : public gtsam::NoiseModelFactorN<VALUE> {
private:
VALUE prior_;
public:
MyPrior(gtsam::Key key, const VALUE &prior,
const gtsam::SharedNoiseModel &model)
: gtsam::NoiseModelFactorN<VALUE>(model, key), prior_(prior) {}
gtsam::Vector evaluateError(
const VALUE &val,
boost::optional<gtsam::Matrix &> H = boost::none) const override {
if (H)
(*H) = gtsam::Matrix::Identity(gtsam::traits<VALUE>::GetDimension(val),
gtsam::traits<VALUE>::GetDimension(val));
// manifold equivalent of z-x -> Local(x,z)
return -gtsam::traits<VALUE>::Local(val, prior_);
}
virtual gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new MyPrior<VALUE>(*this)));
}
};
template <class VALUE>
class MyPriorPrint : public gtsam::NoiseModelFactorN<VALUE> {
private:
VALUE prior_;
public:
MyPriorPrint(gtsam::Key key, const VALUE &prior,
const gtsam::SharedNoiseModel &model)
: gtsam::NoiseModelFactorN<VALUE>(model, key), prior_(prior) {}
gtsam::Vector evaluateError(
const VALUE &val,
boost::optional<gtsam::Matrix &> H = boost::none) const override {
if (H)
(*H) = gtsam::Matrix::Identity(gtsam::traits<VALUE>::GetDimension(val),
gtsam::traits<VALUE>::GetDimension(val));
// manifold equivalent of z-x -> Local(x,z)
auto error = -gtsam::traits<VALUE>::Local(val, prior_);
val.print();
prior_.print();
return error;
}
virtual gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new MyPriorPrint<VALUE>(*this)));
}
};
TEST(NonlinearFactorGraph, NoPrintSideEffects) {
NonlinearFactorGraph fg;
Values vals;
const auto model = noiseModel::Unit::Create(3);
fg.emplace_shared<MyPrior<Pose2>>(0, Pose2(0, 0, 0), model);
vals.insert(0, Pose2(1, 1, 1));
NonlinearFactorGraph fg_print;
Values vals_print;
fg_print.emplace_shared<MyPriorPrint<Pose2>>(0, Pose2(0, 0, 0), model);
vals_print.insert(0, Pose2(1, 1, 1));
std::cout << "Without Prints:" << std::endl;
GaussNewtonOptimizer optimizer(fg, vals);
optimizer.optimize().print();
std::cout << "With Prints:" << std::endl;
GaussNewtonOptimizer optimizer_print(fg_print, vals_print);
optimizer_print.optimize().print();
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */