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

added check and unit test

release/4.3a0
lcarlone 2020-09-26 16:24:34 -04:00
parent 8cf3bc5059
commit 3734039bf5
2 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/sfm/BinaryMeasurement.h
View File

@ -71,7 +71,10 @@ public:
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
// TODO: make this more general?
void makeNoiseModelRobust(){ void makeNoiseModelRobust(){
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_);
if(!robust) // make robust
this->noiseModel_ = noiseModel::Robust::Create( this->noiseModel_ = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_);
} }

22
gtsam/sfm/tests/testBinaryMeasurement.cpp
View File

@ -36,6 +36,7 @@ static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05));
const Unit3 unit3Measured(Vector3(1, 1, 1)); const Unit3 unit3Measured(Vector3(1, 1, 1));
const Rot3 rot3Measured; const Rot3 rot3Measured;
/* ************************************************************************* */
TEST(BinaryMeasurement, Unit3) { TEST(BinaryMeasurement, Unit3) {
BinaryMeasurement<Unit3> unit3Measurement(kKey1, kKey2, unit3Measured, BinaryMeasurement<Unit3> unit3Measurement(kKey1, kKey2, unit3Measured,
unit3_model); unit3_model);
@ -48,6 +49,7 @@ TEST(BinaryMeasurement, Unit3) {
EXPECT(unit3Measurement.equals(unit3MeasurementCopy)); EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
} }
/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3) { TEST(BinaryMeasurement, Rot3) {
// testing the accessors // testing the accessors
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured, BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
@ -62,6 +64,26 @@ TEST(BinaryMeasurement, Rot3) {
EXPECT(rot3Measurement.equals(rot3MeasurementCopy)); EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
} }
/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3MakeRobust) {
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
rot3_model);
rot3Measurement.makeNoiseModelRobust();
EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
EXPECT(rot3Measurement.measured().equals(rot3Measured));
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
rot3Measurement.noiseModel());
EXPECT(robust);
// test that if we call it again nothing changes:
rot3Measurement.makeNoiseModelRobust();
const auto &robust2 = boost::dynamic_pointer_cast<noiseModel::Robust>(
rot3Measurement.noiseModel());
EXPECT(robust2);
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;