diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h index 7851c8be4..51bdd159e 100644 --- a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h @@ -180,7 +180,7 @@ bool Fixed3DLandmarkCostFunctor::operator()(const T* const position, const T* co T fy = calibration[1]; for (uint i = 0; i < pts3d_.cols(); i++) { - // Get the covariance weigthing to point losses from a pose uncertainty + // Get the covariance weighting to point losses from a pose uncertainty // From https://arxiv.org/pdf/2103.15980.pdf , equation A.7: // dh( e A p ) = dh(p') * d(e A p) // d(e) d(p') d(e) @@ -188,7 +188,7 @@ bool Fixed3DLandmarkCostFunctor::operator()(const T* const position, const T* co // h is the projection function, and p' = Ap = g, the jacobian is thus 2x6: // J = // [ (fx/gz) (0) (-fx * gx / gz^2) (-fx * gx gy / gz^2) fx(1+gx^2/gz^2) -fx gy/gz] - // [ 0 (fy/gz)) (-fy * gy / gz^2) -fy(1+gy^2/gz^2) (-fy * gx gy / gz^2) -fy gx/gz] + // [ 0 (fy/gz)) (-fy * gy / gz^2) -fy(1+gy^2/gz^2) (fy * gx gy / gz^2) fy gx/gz] T gx = pts3d_.cast().col(i)[0]; T gy = pts3d_.cast().col(i)[1]; T gz = pts3d_.cast().col(i)[2]; diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h index ccec9a91d..7a55306b2 100644 --- a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h @@ -140,7 +140,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint /** * @brief Read-only access to the square root information matrix. * - * Order is (x, y, z, qx, qy, qz) + * Order is (u, v) */ const fuse_core::Matrix2d& sqrtInformation() const { @@ -150,7 +150,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint /** * @brief Compute the measurement covariance matrix. * - * Order is (x, y, z, qx, qy, qz) + * Order is (u, v) */ fuse_core::Matrix2d covariance() const { @@ -160,7 +160,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint /** * @brief Read-only access to the observation Matrix (Nx2). * - * Order is (x, y) + * Order is (x, y, z) */ const fuse_core::MatrixXd& pts3d() const { @@ -170,7 +170,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint /** * @brief Read-only access to the observation Matrix (Nx2). * - * Order is (x, y) + * Order is (u, v) */ const fuse_core::MatrixXd& observations() const { diff --git a/fuse_constraints/include/fuse_constraints/reprojection_error_cost_functor.h b/fuse_constraints/include/fuse_constraints/reprojection_error_cost_functor.h index 45a6922f7..7c1a3e8b8 100644 --- a/fuse_constraints/include/fuse_constraints/reprojection_error_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/reprojection_error_cost_functor.h @@ -60,7 +60,7 @@ namespace fuse_constraints * where, the matrix A and the vector b are fixed, p is the camera position variable, and q is the camera orientation * variable, K is the calibration matrix created from the calibration variable, X is the set of marker 3D points, * R_b(0:3) is the Rotation matrix from the fixed landmark orentation (b(3:6)), b(0:2) is the fixed landmark position - * and x is the 2D ovservations. + * and x is the 2D observations. * * Note that the covariance submatrix for the quaternion is 3x3, representing errors in the orientation local * parameterization tangent space. In case the user is interested in implementing a cost function of the form @@ -80,7 +80,7 @@ class ReprojectionErrorCostFunctor * * @param[in] A The residual weighting matrix, most likely derived from the square root information * matrix in order (u, v) - * @param[in] b The 2D pose measurement or prior in order (u, v + * @param[in] b The 2D pose measurement or prior in order (u, v) * **/ ReprojectionErrorCostFunctor(const fuse_core::Matrix2d& A, const fuse_core::Vector2d& b); diff --git a/fuse_constraints/include/fuse_constraints/reprojection_error_snavelly_constraint.h b/fuse_constraints/include/fuse_constraints/reprojection_error_snavelly_constraint.h index 236877463..ab2fbf0cd 100644 --- a/fuse_constraints/include/fuse_constraints/reprojection_error_snavelly_constraint.h +++ b/fuse_constraints/include/fuse_constraints/reprojection_error_snavelly_constraint.h @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include @@ -91,7 +91,7 @@ class ReprojectionErrorSnavellyConstraint : public fuse_core::Constraint */ ReprojectionErrorSnavellyConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, const fuse_variables::Orientation3DStamped& orientation, - const fuse_variables::PinholeCameraSimple& calibraton, + const fuse_variables::PinholeCameraRadial& calibraton, const fuse_core::Vector2d& mean, const fuse_core::Matrix2d& covariance); /** diff --git a/fuse_constraints/src/reprojection_error_snavelly_constraint.cpp b/fuse_constraints/src/reprojection_error_snavelly_constraint.cpp index 6db9f2e21..d54c2e739 100644 --- a/fuse_constraints/src/reprojection_error_snavelly_constraint.cpp +++ b/fuse_constraints/src/reprojection_error_snavelly_constraint.cpp @@ -51,7 +51,7 @@ namespace fuse_constraints ReprojectionErrorSnavellyConstraint::ReprojectionErrorSnavellyConstraint( const std::string& source, const fuse_variables::Position3DStamped& position, const fuse_variables::Orientation3DStamped& orientation, - const fuse_variables::PinholeCameraSimple& calibration, + const fuse_variables::PinholeCameraRadial& calibration, const fuse_core::Vector2d& mean, const fuse_core::Matrix2d& covariance) : fuse_core::Constraint(source, { position.uuid(), orientation.uuid(), calibration.uuid() }) diff --git a/fuse_constraints/test/test_bal_problem.cpp b/fuse_constraints/test/test_bal_problem.cpp index 5949b933c..591cb6161 100644 --- a/fuse_constraints/test/test_bal_problem.cpp +++ b/fuse_constraints/test/test_bal_problem.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include @@ -58,7 +58,7 @@ using fuse_constraints::ReprojectionErrorSnavellyConstraint; using fuse_variables::Orientation3DStamped; -using fuse_variables::PinholeCameraSimple; +using fuse_variables::PinholeCameraRadial; using fuse_variables::Point3DFixedLandmark; using fuse_variables::Point3DLandmark; using fuse_variables::Position3DStamped; @@ -230,7 +230,7 @@ struct SnavelyReprojectionErrorWithQuaternions template bool operator()(const T* const camera, const T* const point, T* residuals) const { - // camera[0,1,2,3] is are the rotation of the camera as a quaternion. + // camera[0,1,2,3] is the rotation of the camera as a quaternion. // // We use QuaternionRotatePoint as it does not assume that the // quaternion is normalized, since one of the ways to run the @@ -276,7 +276,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Constructor) Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); Point3DLandmark point(0); - PinholeCameraSimple calibration_variable(0); + PinholeCameraRadial calibration_variable(0); fuse_core::Vector2d mean; mean << 320.0, 240.0; // Centre of a 640x480 camera @@ -296,7 +296,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Covariance) Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); Point3DLandmark point(0); - PinholeCameraSimple calibration_variable(0); + PinholeCameraRadial calibration_variable(0); fuse_core::Vector2d mean; mean << 320.0, 240.0; // Centre of a 640x480 camera @@ -355,7 +355,7 @@ TEST(ReprojectionErrorSnavellyConstraint, BAL) std::vector cams_p(bal_problem.num_cameras()); std::vector cams_q(bal_problem.num_cameras()); - std::vector cams_k(bal_problem.num_cameras()); + std::vector cams_k(bal_problem.num_cameras()); for (int i = 0; i < bal_problem.num_cameras(); i++) { auto cam = bal_problem.camera(i); @@ -456,7 +456,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Serialization) Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); - PinholeCameraSimple calibration_variable(0); + PinholeCameraRadial calibration_variable(0); calibration_variable.f() = 640; calibration_variable.r1() = 0.1; calibration_variable.r2() = 0.1; diff --git a/fuse_constraints/test/test_reprojection_error_snavelly_constraint.cpp b/fuse_constraints/test/test_reprojection_error_snavelly_constraint.cpp index c576d6d6e..973a1caa7 100644 --- a/fuse_constraints/test/test_reprojection_error_snavelly_constraint.cpp +++ b/fuse_constraints/test/test_reprojection_error_snavelly_constraint.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include @@ -55,7 +55,7 @@ using fuse_constraints::ReprojectionErrorSnavellyConstraint; using fuse_variables::Orientation3DStamped; -using fuse_variables::PinholeCameraSimple; +using fuse_variables::PinholeCameraRadial; using fuse_variables::Point3DLandmark; using fuse_variables::Point3DFixedLandmark; using fuse_variables::Position3DStamped; @@ -66,7 +66,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Constructor) Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); Point3DLandmark point(0); - PinholeCameraSimple calibration_variable(0); + PinholeCameraRadial calibration_variable(0); fuse_core::Vector2d mean; mean << 320.0, 240.0; // Centre of a 640x480 camera @@ -86,7 +86,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Covariance) Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); Point3DLandmark point(0); - PinholeCameraSimple calibration_variable(0); + PinholeCameraRadial calibration_variable(0); fuse_core::Vector2d mean; mean << 320.0, 240.0; // Centre of a 640x480 camera @@ -125,7 +125,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Optimization) orientation_variable->y() = -0.189; orientation_variable->z() = 0.239; - auto calibration_variable = PinholeCameraSimple::make_shared(0); + auto calibration_variable = PinholeCameraRadial::make_shared(0); calibration_variable->f() = 638.34478759765620; calibration_variable->r1() = 0.13281739520782995; calibration_variable->r2() = -0.17255676937880005; @@ -263,7 +263,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Serialization) Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); - PinholeCameraSimple calibration_variable(0); + PinholeCameraRadial calibration_variable(0); calibration_variable.f() = 638.34478759765620; calibration_variable.r1() = 0.13281739520782995; calibration_variable.r2() = -0.17255676937880005; diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt index 6c4e0dae1..79b93e1cc 100644 --- a/fuse_variables/CMakeLists.txt +++ b/fuse_variables/CMakeLists.txt @@ -36,7 +36,7 @@ add_library(${PROJECT_NAME} src/acceleration_linear_3d_stamped.cpp src/pinhole_camera.cpp src/pinhole_camera_fixed.cpp - src/pinhole_camera_simple.cpp + src/pinhole_camera_radial.cpp src/orientation_2d_stamped.cpp src/orientation_3d_stamped.cpp src/point_2d_fixed_landmark.cpp @@ -248,24 +248,24 @@ if(CATKIN_ENABLE_TESTING) ) # Camera Intrinsics tests - catkin_add_gtest(test_pinhole_camera_simple - test/test_pinhole_camera_simple.cpp + catkin_add_gtest(test_pinhole_camera_radial + test/test_pinhole_camera_radial.cpp ) - add_dependencies(test_pinhole_camera_simple + add_dependencies(test_pinhole_camera_radial ${catkin_EXPORTED_TARGETS} ) - target_include_directories(test_pinhole_camera_simple + target_include_directories(test_pinhole_camera_radial PRIVATE include ${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ) - target_link_libraries(test_pinhole_camera_simple + target_link_libraries(test_pinhole_camera_radial ${PROJECT_NAME} ${catkin_LIBRARIES} ${CERES_LIBRARIES} ) - set_target_properties(test_pinhole_camera_simple + set_target_properties(test_pinhole_camera_radial PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES diff --git a/fuse_variables/include/fuse_variables/base_camera.h b/fuse_variables/include/fuse_variables/base_camera.h index 862c79713..ddb9ea72e 100644 --- a/fuse_variables/include/fuse_variables/base_camera.h +++ b/fuse_variables/include/fuse_variables/base_camera.h @@ -71,6 +71,7 @@ class BaseCamera : public FixedSizeVariable /** * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters * + * @param[in] uuid The UUID of the sensor * @param[in] camera_id The id associated to a camera */ explicit BaseCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id): @@ -79,10 +80,11 @@ class BaseCamera : public FixedSizeVariable /** * @brief Construct a pinhole camera variable given a camera id * - * @param[in] camera_id The id associated to a camera + * @param[in] camera_name The id associated to a camera (e.g. which camera on a robot) + * @param[in] device_id The device_id associated to the camera (e.g. which robot) */ - explicit BaseCamera(const uint64_t& camera_id): - BaseCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) {} +explicit BaseCamera(const uint64_t& camera_id, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + : BaseCamera(fuse_core::uuid::generate(detail::type(), camera_id, device_id)) {} /** * @brief Read-only access to the id diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.h b/fuse_variables/include/fuse_variables/pinhole_camera.h index ac1f0ece5..da528d0b9 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera.h +++ b/fuse_variables/include/fuse_variables/pinhole_camera.h @@ -138,7 +138,6 @@ class PinholeCamera : public BaseCamera<4> /** * @brief Read-only access to the id */ - // const uint64_t& id() const { return id_; } /** * @brief Print a human-readable description of the variable to the provided diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_simple.h b/fuse_variables/include/fuse_variables/pinhole_camera_radial.h similarity index 89% rename from fuse_variables/include/fuse_variables/pinhole_camera_simple.h rename to fuse_variables/include/fuse_variables/pinhole_camera_radial.h index 33557e39b..32aaba205 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera_simple.h +++ b/fuse_variables/include/fuse_variables/pinhole_camera_radial.h @@ -34,8 +34,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_SIMPLE_H -#define FUSE_VARIABLES_PINHOLE_CAMERA_SIMPLE_H +#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_RADIAL_H +#define FUSE_VARIABLES_PINHOLE_CAMERA_RADIAL_H #include #include @@ -58,10 +58,10 @@ namespace fuse_variables * construction and dependent on a user input database id. As such, the database id cannot be altered after * construction. */ -class PinholeCameraSimple : public BaseCamera<3> +class PinholeCameraRadial : public BaseCamera<3> { public: - FUSE_VARIABLE_DEFINITIONS(PinholeCameraSimple); + FUSE_VARIABLE_DEFINITIONS(PinholeCameraRadial); /** * @brief Can be used to directly index variables in the data array @@ -76,21 +76,21 @@ class PinholeCameraSimple : public BaseCamera<3> /** * @brief Default constructor */ - PinholeCameraSimple() = default; + PinholeCameraRadial() = default; /** * @brief Construct a pinhole camera variable given a camera id * * @param[in] camera_id The id associated to a camera */ - explicit PinholeCameraSimple(const uint64_t& camera_id); + explicit PinholeCameraRadial(const uint64_t& camera_id); /** * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters * * @param[in] camera_id The id associated to a camera */ - explicit PinholeCameraSimple(const fuse_core::UUID& uuid, const uint64_t& camera_id, + explicit PinholeCameraRadial(const fuse_core::UUID& uuid, const uint64_t& camera_id, const double& f, const double& r1, const double& r2); /** @@ -137,7 +137,7 @@ class PinholeCameraSimple : public BaseCamera<3> * * @param[in] camera_id The id associated to a camera_id */ - PinholeCameraSimple(const fuse_core::UUID& uuid, const uint64_t& camera_id); + PinholeCameraRadial(const fuse_core::UUID& uuid, const uint64_t& camera_id); private: // Allow Boost Serialization access to private methods @@ -162,6 +162,6 @@ class PinholeCameraSimple : public BaseCamera<3> } // namespace fuse_variables -BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCameraSimple); +BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCameraRadial); -#endif // FUSE_VARIABLES_PINHOLE_CAMERA_SIMPLE_H +#endif // FUSE_VARIABLES_PINHOLE_CAMERA_RADIAL_H diff --git a/fuse_variables/src/pinhole_camera_simple.cpp b/fuse_variables/src/pinhole_camera_radial.cpp similarity index 82% rename from fuse_variables/src/pinhole_camera_simple.cpp rename to fuse_variables/src/pinhole_camera_radial.cpp index 10a51b528..b0ece9cd3 100644 --- a/fuse_variables/src/pinhole_camera_simple.cpp +++ b/fuse_variables/src/pinhole_camera_radial.cpp @@ -34,7 +34,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include @@ -47,26 +47,26 @@ namespace fuse_variables { -PinholeCameraSimple::PinholeCameraSimple(const fuse_core::UUID& uuid, const uint64_t& camera_id) +PinholeCameraRadial::PinholeCameraRadial(const fuse_core::UUID& uuid, const uint64_t& camera_id) : BaseCamera(uuid, camera_id) { } -PinholeCameraSimple::PinholeCameraSimple(const uint64_t& camera_id) - : PinholeCameraSimple(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) +PinholeCameraRadial::PinholeCameraRadial(const uint64_t& camera_id) + : PinholeCameraRadial(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) { } -PinholeCameraSimple::PinholeCameraSimple(const fuse_core::UUID& uuid, const uint64_t& camera_id, +PinholeCameraRadial::PinholeCameraRadial(const fuse_core::UUID& uuid, const uint64_t& camera_id, const double& f, const double& r1, const double& r2) - : PinholeCameraSimple(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) + : PinholeCameraRadial(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) { data_[F] = f; data_[R1] = r1; data_[R2] = r2; } -void PinholeCameraSimple::print(std::ostream& stream) const +void PinholeCameraRadial::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" @@ -80,5 +80,5 @@ void PinholeCameraSimple::print(std::ostream& stream) const } // namespace fuse_variables -BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::PinholeCameraSimple); -PLUGINLIB_EXPORT_CLASS(fuse_variables::PinholeCameraSimple, fuse_core::Variable); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::PinholeCameraRadial); +PLUGINLIB_EXPORT_CLASS(fuse_variables::PinholeCameraRadial, fuse_core::Variable); diff --git a/fuse_variables/test/test_pinhole_camera_simple.cpp b/fuse_variables/test/test_pinhole_camera_radial.cpp similarity index 87% rename from fuse_variables/test/test_pinhole_camera_simple.cpp rename to fuse_variables/test/test_pinhole_camera_radial.cpp index 530516b01..9a824f321 100644 --- a/fuse_variables/test/test_pinhole_camera_simple.cpp +++ b/fuse_variables/test/test_pinhole_camera_radial.cpp @@ -35,7 +35,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include #include @@ -47,27 +47,27 @@ #include #include -using fuse_variables::PinholeCameraSimple; +using fuse_variables::PinholeCameraRadial; -TEST(PinholeCameraSimpleSimple, Type) +TEST(PinholeCameraRadialSimple, Type) { - PinholeCameraSimple variable(0); - EXPECT_EQ("fuse_variables::PinholeCameraSimple", variable.type()); + PinholeCameraRadial variable(0); + EXPECT_EQ("fuse_variables::PinholeCameraRadial", variable.type()); } -TEST(PinholeCameraSimple, UUID) +TEST(PinholeCameraRadial, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - PinholeCameraSimple variable1(0); - PinholeCameraSimple variable2(0); + PinholeCameraRadial variable1(0); + PinholeCameraRadial variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - PinholeCameraSimple variable1(0); - PinholeCameraSimple variable2(1); + PinholeCameraRadial variable1(0); + PinholeCameraRadial variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } @@ -89,10 +89,10 @@ struct CostFunctor } }; -TEST(PinholeCameraSimple, Optimization) +TEST(PinholeCameraRadial, Optimization) { // Create a Point3DLandmark - PinholeCameraSimple K(0); + PinholeCameraRadial K(0); K.f() = 4.1; K.r1() = 3.5; K.r2() = 5; @@ -119,10 +119,10 @@ TEST(PinholeCameraSimple, Optimization) EXPECT_NEAR(0.51, K.r2(), 1.0e-5); } -TEST(PinholeCameraSimple, Serialization) +TEST(PinholeCameraRadial, Serialization) { // Create a Point3DLandmark - PinholeCameraSimple expected(0); + PinholeCameraRadial expected(0); expected.f() = 640; expected.r1() = 0.5; expected.r2() = 0.5; @@ -135,7 +135,7 @@ TEST(PinholeCameraSimple, Serialization) } // Deserialize a new variable from that same stream - PinholeCameraSimple actual; + PinholeCameraRadial actual; { fuse_core::TextInputArchive archive(stream); actual.deserialize(archive);