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

changes with passing unit tests

release/4.3a0
akrishnan86 2020-08-05 00:18:42 -07:00
parent 851d4a4af4
commit ae5956bd10
2 changed files with 30 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/sfm/BinaryMeasurement.h
View File

@ -18,6 +18,13 @@
* @brief Binary measurement for representing edges in the epipolar graph * @brief Binary measurement for representing edges in the epipolar graph
*/ */
#include <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
@ -71,22 +78,17 @@ public:
} }
/** equals */ /** equals */
bool equals(const BetweenMeasurement& expected, double tol=1e-9) const { bool equals(const BinaryMeasurement& expected, double tol=1e-9) const {
const BetweenMeasurement<VALUE> *e = dynamic_cast<const BetweenMeasurement<VALUE>*> (&expected); const BinaryMeasurement<VALUE> *e = dynamic_cast<const BinaryMeasurement<VALUE>*> (&expected);
return e != nullptr && key1_ == expected.key1() && return e != nullptr && key1_ == e->key1_ &&
key2_ == expected.key2() key2_ == e->key2_
&& traits<VALUE>::Equals(this->measured_, e->measured_, tol) && && traits<VALUE>::Equals(this->measured_, e->measured_, tol) &&
noiseModel_.equals(expected.noiseModel()); noiseModel_->equals(expected.noiseModel());
} }
/** return the measured */ /** return the measured */
const VALUE& measured() const { VALUE measured() const {
return measured_; return measured_;
} }
/** number of variables attached to this measurement */
std::size_t size() const {
return 2;
}
}; // \class BetweenMeasurement }; // \class BetweenMeasurement
} }

26
tests/testTranslationRecovery.cpp
View File

@ -49,15 +49,17 @@ TEST(TranslationRecovery, BAL) {
poses, {{0, 1}, {0, 2}, {1, 2}}); poses, {{0, 1}, {0, 2}, {1, 2}});
// Check // Check
const Pose3 wTa = poses.at<Pose3>(0), wTb = poses.at<Pose3>(1), Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test
wTc = poses.at<Pose3>(2); for(auto& unitTranslation : relativeTranslations) {
const Point3 Ta = wTa.translation(), Tb = wTb.translation(), const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()),
Tc = wTc.translation(); wTb = poses.at<Pose3>(unitTranslation.key2());
const Rot3 aRw = wTa.rotation().inverse(); const Point3 Ta = wTa.translation(), Tb = wTb.translation();
const Unit3 w_aZb = relativeTranslations.at({0, 1}); const Unit3 w_aZb = unitTranslation.measured();
EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
const Unit3 w_aZc = relativeTranslations.at({0, 2}); if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) {
EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc)); w_aZb_stored = unitTranslation.measured();
}
}
TranslationRecovery algorithm(relativeTranslations); TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph(); const auto graph = algorithm.buildGraph();
@ -69,10 +71,14 @@ TEST(TranslationRecovery, BAL) {
// Check result for first two translations, determined by prior // Check result for first two translations, determined by prior
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0))); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at<Point3>(1))); EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at<Point3>(1)));
// Check that the third translations is correct // Check that the third translations is correct
Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); Point3 Ta = poses.at<Pose3>(0).translation();
Point3 Tb = poses.at<Pose3>(1).translation();
Point3 Tc = poses.at<Pose3>(2).translation();
Point3 expected =
(Tc - Ta) * (scale / (Tb - Ta).norm());
EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4)); EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
// TODO(frank): how to get stats back? // TODO(frank): how to get stats back?