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

28
tests/testTranslationRecovery.cpp
View File

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