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

Test 3D version

release/4.3a0
Frank Dellaert 2015-07-11 16:56:38 -07:00
parent 58a280c672
commit e40c546931
1 changed files with 27 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/sam/tests/testBearingFactor.cpp
View File

@ -28,18 +28,18 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5));
Key poseKey(1); Key poseKey(1);
Key pointKey(2); Key pointKey(2);
typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D; typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
double measurement2D(10.0); double measurement2D(10.0);
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model); static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
GTSAM_CONCEPT_TESTABLE_INST(BearingFactor2D) BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D; typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
Unit3 measurement3D(1,2,3);
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
/* ************************************************************************* */ /* ************************************************************************* */
// Export Noisemodels // Export Noisemodels
@ -74,21 +74,28 @@ TEST(BearingFactor, 2D) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO(frank): this is broken: (non-existing) Pose3::bearing should yield a TEST(BearingFactor, Serialization3D) {
// Unit3 EXPECT(serializationTestHelpers::equalsObj<BearingFactor3D>(factor3D));
// TEST(BearingFactor, 3D) { EXPECT(serializationTestHelpers::equalsXML<BearingFactor3D>(factor3D));
// // Create a factor EXPECT(serializationTestHelpers::equalsBinary<BearingFactor3D>(factor3D));
// Rot3 measurement; }
// BearingFactor<Pose3, Point3> factor(poseKey, pointKey, measurement, model);
// /* ************************************************************************* */
// // Set the linearization point TEST(BearingFactor, 3D) {
// Values values; // Serialize the factor
// values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, std::string serialized = serializeXML(factor3D);
// -3.0)));
// values.insert(pointKey, Point3(-2.0, 11.0, 1.0)); // And de-serialize it
// BearingFactor3D factor;
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); deserializeXML(serialized, factor);
//}
// Set the linearization point
Values values;
values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0,-3.0)));
values.insert(pointKey, Point3(-2.0, 11.0, 1.0));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {