commit
de4aa2ab93
|
|
@ -57,7 +57,7 @@ static StereoCamera cam2(pose3, cal4ptr);
|
||||||
static StereoPoint2 spt(1.0, 2.0, 3.0);
|
static StereoPoint2 spt(1.0, 2.0, 3.0);
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST_DISABLED (Serialization, text_geometry) {
|
TEST (Serialization, text_geometry) {
|
||||||
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||||
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||||
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||||
|
|
@ -82,7 +82,7 @@ TEST_DISABLED (Serialization, text_geometry) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST_DISABLED (Serialization, xml_geometry) {
|
TEST (Serialization, xml_geometry) {
|
||||||
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
|
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||||
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||||
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||||
|
|
@ -106,7 +106,7 @@ TEST_DISABLED (Serialization, xml_geometry) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST_DISABLED (Serialization, binary_geometry) {
|
TEST (Serialization, binary_geometry) {
|
||||||
EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0)));
|
EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||||
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||||
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||||
|
|
|
||||||
|
|
@ -65,6 +65,14 @@ class TestPose3(GtsamTestCase):
|
||||||
actual = Pose3.adjoint_(xi, xi)
|
actual = Pose3.adjoint_(xi, xi)
|
||||||
np.testing.assert_array_equal(actual, expected)
|
np.testing.assert_array_equal(actual, expected)
|
||||||
|
|
||||||
|
def test_serialization(self):
|
||||||
|
"""Test if serialization is working normally"""
|
||||||
|
expected = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
|
||||||
|
actual = Pose3()
|
||||||
|
serialized = expected.serialize()
|
||||||
|
actual.deserialize(serialized)
|
||||||
|
self.gtsamAssertEquals(expected, actual, 1e-10)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
unittest.main()
|
unittest.main()
|
||||||
|
|
|
||||||
|
|
@ -69,13 +69,13 @@ class PybindWrapper(object):
|
||||||
return textwrap.dedent('''
|
return textwrap.dedent('''
|
||||||
.def("serialize",
|
.def("serialize",
|
||||||
[]({class_inst} self){{
|
[]({class_inst} self){{
|
||||||
return gtsam::serialize(self);
|
return gtsam::serialize(*self);
|
||||||
}}
|
}}
|
||||||
)
|
)
|
||||||
.def("deserialize",
|
.def("deserialize",
|
||||||
[]({class_inst} self, string serialized){{
|
[]({class_inst} self, string serialized){{
|
||||||
return gtsam::deserialize(serialized, self);
|
gtsam::deserialize(serialized, *self);
|
||||||
}})
|
}}, py::arg("serialized"))
|
||||||
'''.format(class_inst=cpp_class + '*'))
|
'''.format(class_inst=cpp_class + '*'))
|
||||||
|
|
||||||
is_method = isinstance(method, instantiator.InstantiatedMethod)
|
is_method = isinstance(method, instantiator.InstantiatedMethod)
|
||||||
|
|
|
||||||
|
|
@ -40,13 +40,13 @@ PYBIND11_MODULE(geometry_py, m_) {
|
||||||
.def("vectorConfusion",[](gtsam::Point2* self){return self->vectorConfusion();})
|
.def("vectorConfusion",[](gtsam::Point2* self){return self->vectorConfusion();})
|
||||||
.def("serialize",
|
.def("serialize",
|
||||||
[](gtsam::Point2* self){
|
[](gtsam::Point2* self){
|
||||||
return gtsam::serialize(self);
|
return gtsam::serialize(*self);
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
.def("deserialize",
|
.def("deserialize",
|
||||||
[](gtsam::Point2* self, string serialized){
|
[](gtsam::Point2* self, string serialized){
|
||||||
return gtsam::deserialize(serialized, self);
|
gtsam::deserialize(serialized, *self);
|
||||||
})
|
}, py::arg("serialized"))
|
||||||
;
|
;
|
||||||
|
|
||||||
py::class_<gtsam::Point3, std::shared_ptr<gtsam::Point3>>(m_gtsam, "Point3")
|
py::class_<gtsam::Point3, std::shared_ptr<gtsam::Point3>>(m_gtsam, "Point3")
|
||||||
|
|
@ -54,13 +54,13 @@ PYBIND11_MODULE(geometry_py, m_) {
|
||||||
.def("norm",[](gtsam::Point3* self){return self->norm();})
|
.def("norm",[](gtsam::Point3* self){return self->norm();})
|
||||||
.def("serialize",
|
.def("serialize",
|
||||||
[](gtsam::Point3* self){
|
[](gtsam::Point3* self){
|
||||||
return gtsam::serialize(self);
|
return gtsam::serialize(*self);
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
.def("deserialize",
|
.def("deserialize",
|
||||||
[](gtsam::Point3* self, string serialized){
|
[](gtsam::Point3* self, string serialized){
|
||||||
return gtsam::deserialize(serialized, self);
|
gtsam::deserialize(serialized, *self);
|
||||||
})
|
}, py::arg("serialized"))
|
||||||
|
|
||||||
.def_static("staticFunction",[](){return gtsam::Point3::staticFunction();})
|
.def_static("staticFunction",[](){return gtsam::Point3::staticFunction();})
|
||||||
.def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z"));
|
.def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z"));
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue