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

create backwards compatibility unit test for NoiseModelFactor1

release/4.3a0
Gerry Chen 2022-01-30 17:23:02 -05:00
parent 3addc8dfff
commit c9dbb6e040
No known key found for this signature in database
GPG Key ID: E9845092D3A57286
2 changed files with 130 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
gtsam/nonlinear/tests/priorFactor.xml Normal file
View File

@ -0,0 +1,77 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<!DOCTYPE boost_serialization>
<boost_serialization signature="serialization::archive" version="19">
<data class_id="0" tracking_level="1" version="0" object_id="_0">
<NoiseModelFactor1 class_id="1" tracking_level="0" version="0">
<NoiseModelFactor class_id="2" tracking_level="0" version="0">
<NonlinearFactor class_id="3" tracking_level="0" version="0">
<keys_>
<count>1</count>
<item_version>0</item_version>
<item>12345</item>
</keys_>
</NonlinearFactor>
<noiseModel_ class_id="5" tracking_level="0" version="1">
<px class_id="6" class_name="gtsam_noiseModel_Unit" tracking_level="1" version="0" object_id="_1">
<Isotropic class_id="7" tracking_level="1" version="0" object_id="_2">
<Diagonal class_id="8" tracking_level="1" version="0" object_id="_3">
<Gaussian class_id="9" tracking_level="0" version="0">
<Base class_id="10" tracking_level="0" version="0">
<dim_>6</dim_>
</Base>
<sqrt_information_ class_id="11" tracking_level="0" version="1">
<initialized>0</initialized>
</sqrt_information_>
</Gaussian>
<sigmas_ class_id="12" tracking_level="0" version="0">
<size>6</size>
<data>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
</data>
</sigmas_>
<invsigmas_>
<size>6</size>
<data>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
</data>
</invsigmas_>
</Diagonal>
<sigma_>1.00000000000000000e+00</sigma_>
<invsigma_>1.00000000000000000e+00</invsigma_>
</Isotropic>
</px>
</noiseModel_>
</NoiseModelFactor>
</NoiseModelFactor1>
<prior_ class_id="13" tracking_level="0" version="0">
<R_ class_id="14" tracking_level="0" version="0">
<rot11>4.11982245665682978e-01</rot11>
<rot12>-8.33737651774156818e-01</rot12>
<rot13>-3.67630462924899259e-01</rot13>
<rot21>-5.87266449276209815e-02</rot21>
<rot22>-4.26917621276207360e-01</rot22>
<rot23>9.02381585483330806e-01</rot23>
<rot31>-9.09297426825681709e-01</rot31>
<rot32>-3.50175488374014632e-01</rot32>
<rot33>-2.24845095366152908e-01</rot33>
</R_>
<t_ class_id="15" tracking_level="0" version="0">
<data>
<item>4.00000000000000000e+00</item>
<item>5.00000000000000000e+00</item>
<item>6.00000000000000000e+00</item>
</data>
</t_>
</prior_>
</data>
</boost_serialization>

53
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -107,6 +107,59 @@ TEST (Serialization, TemplatedValues) {
EXPECT(equalsBinary(values));
}
bool str_assert_equal(const string& expected, const string& actual) {
if (actual == expected) return true;
printf("Not equal:\n");
std::cout << "expected:\n" << expected << std::endl;
std::cout << "actual:\n" << actual << std::endl;
return false;
}
TEST(Serialization, NoiseModelFactor1_backwards_compatibility) {
PriorFactor<Pose3> factor(
12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)),
noiseModel::Unit::Create(6));
// String
std::string actual_str = serialize(factor);
// Generated from hash 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4)
std::string expected_str =
"22 serialization::archive 19 1 0\n"
"0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n"
"1 1 0\n"
"2 1 0\n"
"3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 6 1.00000000000000000e+00 "
"1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
"1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 "
"-8.33737651774156818e-01 -3.67630462924899259e-01 "
"-5.87266449276209815e-02 -4.26917621276207360e-01 "
"9.02381585483330806e-01 -9.09297426825681709e-01 "
"-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 "
"4.00000000000000000e+00 5.00000000000000000e+00 "
"6.00000000000000000e+00\n";
EXPECT(str_assert_equal(expected_str, actual_str));
PriorFactor<Pose3> factor_deserialized_str = PriorFactor<Pose3>();
deserializeFromString(expected_str, factor_deserialized_str);
EXPECT(assert_equal(factor, factor_deserialized_str));
// XML
std::string actual_xml = serializeXML(factor);
std::string expected_xml;
{ // read from file
// Generated from hash 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4)
std::ifstream f("priorFactor.xml");
std::stringstream buffer;
buffer << f.rdbuf();
expected_xml = buffer.str();
}
EXPECT(str_assert_equal(expected_xml, actual_xml));
PriorFactor<Pose3> factor_deserialized_xml = PriorFactor<Pose3>();
deserializeFromXMLFile("priorFactor.xml", factor_deserialized_xml);
EXPECT(assert_equal(factor, factor_deserialized_xml));
}
TEST(Serialization, ISAM2) {
gtsam::ISAM2Params parameters;