diff --git a/cpp/bindings/main.cpp b/cpp/bindings/main.cpp index 8397dd06..0cc6e6c2 100644 --- a/cpp/bindings/main.cpp +++ b/cpp/bindings/main.cpp @@ -21,13 +21,13 @@ NB_MODULE(_cpp, m) { "types", "Common types used for conversion between datasets and pipelines." ); - evalio::makeTypes(m_types); + evalio::make_types(m_types); auto m_helpers = m.def_submodule("helpers", "Helper functions for internal evalio usage."); - evalio::makeConversions(m_helpers); + evalio::make_conversions(m_helpers); auto m_pipelines = m.def_submodule("pipelines", "Pipelines used in evalio."); - evalio::makeBasePipeline(m_pipelines); - evalio::makePipelines(m_pipelines); + evalio::make_base_pipeline(m_pipelines); + evalio::make_pipelines(m_pipelines); } diff --git a/cpp/bindings/pipeline.h b/cpp/bindings/pipeline.h index a2ea97b7..bded43a1 100644 --- a/cpp/bindings/pipeline.h +++ b/cpp/bindings/pipeline.h @@ -57,7 +57,7 @@ class PyPipeline: public evalio::Pipeline { } }; // namespace evalio -inline void makeBasePipeline(nb::module_& m) { +inline void make_base_pipeline(nb::module_& m) { nb::class_(m, "Pipeline") .def(nb::init<>(), "Construct a new pipeline.") .def_static("name", &evalio::Pipeline::name, "Name of the pipeline.") diff --git a/cpp/bindings/pipelines/bindings.h b/cpp/bindings/pipelines/bindings.h index eab6f5ae..508016a7 100644 --- a/cpp/bindings/pipelines/bindings.h +++ b/cpp/bindings/pipelines/bindings.h @@ -32,7 +32,7 @@ using namespace nb::literals; #endif namespace evalio { -inline void makePipelines(nb::module_& m) { +inline void make_pipelines(nb::module_& m) { // List all the pipelines here #ifdef EVALIO_KISS_ICP nb::class_(m, "KissICP") diff --git a/cpp/bindings/pipelines/ct_icp.h b/cpp/bindings/pipelines/ct_icp.h index 5c77a46e..911ed455 100644 --- a/cpp/bindings/pipelines/ct_icp.h +++ b/cpp/bindings/pipelines/ct_icp.h @@ -39,7 +39,7 @@ inline ct_icp::Point3D convert(const evalio::Point& from) { template<> inline evalio::SE3 convert(const ct_icp::TrajectoryFrame& in) { - return ev::SE3(ev::SO3::fromMat(in.begin_R), in.begin_t); + return ev::SE3(ev::SO3::from_mat(in.begin_R), in.begin_t); } } // namespace evalio diff --git a/cpp/bindings/pipelines/lio_sam.h b/cpp/bindings/pipelines/lio_sam.h index b5b91caa..ab5dd111 100644 --- a/cpp/bindings/pipelines/lio_sam.h +++ b/cpp/bindings/pipelines/lio_sam.h @@ -140,7 +140,7 @@ class LioSam: public ev::Pipeline { void set_imu_T_lidar(ev::SE3 T) override { lidar_T_imu_ = T.inverse(); config_.lidar_P_imu = lidar_T_imu_.trans; - config_.lidar_R_imu = lidar_T_imu_.rot.toEigen(); + config_.lidar_R_imu = lidar_T_imu_.rot.to_eigen(); } // Doers diff --git a/cpp/bindings/pipelines/loam.h b/cpp/bindings/pipelines/loam.h index 045ed016..ddcc4aa1 100644 --- a/cpp/bindings/pipelines/loam.h +++ b/cpp/bindings/pipelines/loam.h @@ -13,7 +13,7 @@ namespace ev = evalio; namespace evalio { template<> inline ev::SE3 convert(const loam::Pose3d& from) { - return ev::SE3(ev::SO3::fromEigen(from.rotation), from.translation); + return ev::SE3(ev::SO3::from_eigen(from.rotation), from.translation); } } // namespace evalio diff --git a/cpp/bindings/ros_pc2.h b/cpp/bindings/ros_pc2.h index bcd0a8f7..0dc62061 100644 --- a/cpp/bindings/ros_pc2.h +++ b/cpp/bindings/ros_pc2.h @@ -468,7 +468,7 @@ inline bool closest(const Stamp& idx, const Stamp& a, const Stamp& b) { } // ---------------------- Create python bindings ---------------------- // -inline void makeConversions(nb::module_& m) { +inline void make_conversions(nb::module_& m) { nb::enum_(m, "DataType") .value("UINT8", DataType::UINT8) .value("INT8", DataType::INT8) diff --git a/cpp/bindings/types.h b/cpp/bindings/types.h index a3855f05..0985c00e 100644 --- a/cpp/bindings/types.h +++ b/cpp/bindings/types.h @@ -20,7 +20,7 @@ namespace evalio { // TODO: Check if copy/deepcopy works or not -inline void makeTypes(nb::module_& m) { +inline void make_types(nb::module_& m) { nb::enum_(m, "VisOption", nb::is_flag()) .value("MAP", VisOption::MAP, "Visualize the map.") .value("FEATURES", VisOption::FEATURES, "Visualize the features.") @@ -58,7 +58,7 @@ inline void makeTypes(nb::module_& m) { ) .def(nb::self - nb::self, "Compute the difference between two Durations") .def(nb::self + nb::self, "Add two Durations") - .def("__repr__", &Duration::toString) + .def("__repr__", &Duration::to_string) .def("__copy__", [](const Duration& self) { return Duration(self); }) .def( "__deepcopy__", @@ -124,7 +124,7 @@ inline void makeTypes(nb::module_& m) { ) .def(nb::self + Duration(), "Add a Duration to a Stamp") .def(nb::self - Duration(), "Subtract a Duration from a Stamp") - .def("__repr__", &Stamp::toString) + .def("__repr__", &Stamp::to_string) .def("__copy__", [](const Stamp& self) { return Stamp(self); }) .def( "__deepcopy__", @@ -200,7 +200,7 @@ inline void makeTypes(nb::module_& m) { "Check for inequality", nb::sig("def __ne__(self, arg: object, /) -> bool") ) - .def("__repr__", &Point::toString) + .def("__repr__", &Point::to_string) .def( "__getstate__", [](const Point& p) { @@ -280,7 +280,7 @@ inline void makeTypes(nb::module_& m) { "Check for inequality", nb::sig("def __ne__(self, arg: object, /) -> bool") ) - .def("__repr__", &LidarMeasurement::toString) + .def("__repr__", &LidarMeasurement::to_string) .def( "__getstate__", [](const LidarMeasurement& p) { @@ -342,7 +342,7 @@ inline void makeTypes(nb::module_& m) { "Get the time between two consecutive scans as a Duration. Inverse " "of the rate." ) - .def("__repr__", &LidarParams::toString) + .def("__repr__", &LidarParams::to_string) .doc() = "LidarParams is a structure for storing the parameters of a " "lidar sensor."; @@ -380,7 +380,7 @@ inline void makeTypes(nb::module_& m) { "Check for inequality", nb::sig("def __ne__(self, arg: object, /) -> bool") ) - .def("__repr__", &ImuMeasurement::toString) + .def("__repr__", &ImuMeasurement::to_string) .def( "__getstate__", [](const ImuMeasurement& p) { @@ -473,7 +473,7 @@ inline void makeTypes(nb::module_& m) { .def_ro("rate", &ImuParams::rate, "Rate of the IMU sensor, in Hz.") .def_ro("brand", &ImuParams::brand, "Brand of the IMU sensor.") .def_ro("model", &ImuParams::model, "Model of the IMU sensor.") - .def("__repr__", &ImuParams::toString) + .def("__repr__", &ImuParams::to_string) .doc() = "ImuParams is a structure for storing the parameters of an IMU"; nb::class_(m, "SO3") @@ -491,15 +491,15 @@ inline void makeTypes(nb::module_& m) { .def_ro("qw", &SO3::qw, "Scalar component of the quaternion.") .def_static("identity", &SO3::identity, "Create an identity rotation.") .def_static( - "fromMat", - &SO3::fromMat, + "from_mat", + &SO3::from_mat, "mat"_a, "Create a rotation from a 3x3 rotation matrix." ) .def_static("exp", &SO3::exp, "v"_a, "Create a rotation from a 3D vector.") .def("inverse", &SO3::inverse, "Compute the inverse of the rotation.") .def("log", &SO3::log, "Compute the logarithm of the rotation.") - .def("toMat", &SO3::toMat, "Convert the rotation to a 3x3 matrix.") + .def("to_mat", &SO3::to_mat, "Convert the rotation to a 3x3 matrix.") .def("rotate", &SO3::rotate, "v"_a, "Rotate a 3D vector by the rotation.") .def(nb::self * nb::self, "Compose two rotations.") .def( @@ -512,7 +512,7 @@ inline void makeTypes(nb::module_& m) { "Check for inequality", nb::sig("def __ne__(self, arg: object, /) -> bool") ) - .def("__repr__", &SO3::toString) + .def("__repr__", &SO3::to_string) .def("__copy__", [](const SO3& self) { return SO3(self); }) .def( "__deepcopy__", @@ -551,14 +551,14 @@ inline void makeTypes(nb::module_& m) { .def(nb::init(), "other"_a, "Copy constructor for SE3.") .def_static("identity", &SE3::identity, "Create an identity SE3.") .def_static( - "fromMat", - &SE3::fromMat, + "from_mat", + &SE3::from_mat, "mat"_a, "Create a SE3 from a 4x4 transformation matrix." ) .def_ro("rot", &SE3::rot, "Rotation as a SO3 object.") .def_ro("trans", &SE3::trans, "Translation as a 3D vector.") - .def("toMat", &SE3::toMat, "Convert to a 4x4 matrix.") + .def("to_mat", &SE3::to_mat, "Convert to a 4x4 matrix.") .def("inverse", &SE3::inverse, "Compute the inverse.") .def_static( "error", @@ -588,7 +588,7 @@ inline void makeTypes(nb::module_& m) { "Check for inequality", nb::sig("def __ne__(self, arg: object, /) -> bool") ) - .def("__repr__", &SE3::toString) + .def("__repr__", &SE3::to_string) .def("__copy__", [](const SE3& self) { return SE3(self); }) .def( "__deepcopy__", diff --git a/cpp/evalio/convert/sophus.h b/cpp/evalio/convert/sophus.h index a57d6a28..19e70182 100644 --- a/cpp/evalio/convert/sophus.h +++ b/cpp/evalio/convert/sophus.h @@ -18,7 +18,7 @@ inline SE3 convert(const Sophus::SE3d& in) { template<> inline Sophus::SE3d convert(const SE3& in) { - return Sophus::SE3d(Sophus::SO3d(in.rot.toEigen()), in.trans); + return Sophus::SE3d(Sophus::SO3d(in.rot.to_eigen()), in.trans); } } // namespace evalio diff --git a/cpp/evalio/types.h b/cpp/evalio/types.h index f5941ab3..c10a51ad 100644 --- a/cpp/evalio/types.h +++ b/cpp/evalio/types.h @@ -36,11 +36,11 @@ struct Duration { return nsec; } - std::string toString() const { - return "Duration(" + toStringBrief() + ")"; + std::string to_string() const { + return "Duration(" + to_string_brief() + ")"; } - std::string toStringBrief() const { + std::string to_string_brief() const { return std::to_string(to_sec()); } @@ -95,11 +95,11 @@ struct Stamp { return double(sec) + double(nsec) * 1e-9; } - std::string toString() const { - return "Stamp(" + toStringBrief() + ")"; + std::string to_string() const { + return "Stamp(" + to_string_brief() + ")"; } - std::string toStringBrief() const { + std::string to_string_brief() const { size_t n_zeros = 9; auto nsec_str = std::to_string(nsec); auto nsec_str_leading = @@ -146,7 +146,7 @@ struct Point { uint8_t row = 0; uint16_t col = 0; - std::string toString() const { + std::string to_string() const { return "Point(x: " + std::to_string(x) + ", y: " + std::to_string(y) + ", z: " + std::to_string(z) + ", intensity: " + std::to_string(intensity) + ", t: " + std::to_string(t.to_sec()) @@ -173,9 +173,9 @@ struct LidarMeasurement { LidarMeasurement(Stamp stamp, std::vector points) : stamp(stamp), points(points) {} - std::string toString() const { + std::string to_string() const { std::ostringstream oss; - oss << "LidarMeasurement(stamp: " << stamp.toStringBrief() + oss << "LidarMeasurement(stamp: " << stamp.to_string_brief() << ", num_points: " << points.size() << ")"; return oss.str(); } @@ -227,7 +227,7 @@ struct LidarParams { std::string brand = "-"; std::string model = "-"; - std::string toString() const { + std::string to_string() const { return "LidarParams(rows: " + std::to_string(num_rows) + ", cols: " + std::to_string(num_columns) + ", min_range: " + std::to_string(min_range) + ", max_range: " + std::to_string(max_range) @@ -244,9 +244,9 @@ struct ImuMeasurement { Eigen::Vector3d gyro; Eigen::Vector3d accel; - std::string toString() const { + std::string to_string() const { std::ostringstream oss; - oss << "ImuMeasurement(stamp: " << stamp.toStringBrief() << ", gyro: [" + oss << "ImuMeasurement(stamp: " << stamp.to_string_brief() << ", gyro: [" << gyro.transpose() << "]" << ", accel: [" << accel.transpose() << "])"; return oss.str(); @@ -285,7 +285,7 @@ struct ImuParams { return imu_params; } - std::string toString() const { + std::string to_string() const { std::ostringstream oss; oss << "ImuParams(gyro: " << gyro << ", accel: " << accel << ", gyro_bias: " << gyro_bias << ", accel_bias: " << accel_bias @@ -302,11 +302,11 @@ struct SO3 { double qz; double qw; - Eigen::Quaterniond toEigen() const { + Eigen::Quaterniond to_eigen() const { return Eigen::Quaterniond(qw, qx, qy, qz); } - static SO3 fromEigen(const Eigen::Quaterniond& q) { + static SO3 from_eigen(const Eigen::Quaterniond& q) { return SO3 {.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; } @@ -314,8 +314,8 @@ struct SO3 { return SO3 {.qx = 0, .qy = 0, .qz = 0, .qw = 1}; } - static SO3 fromMat(const Eigen::Matrix3d& R) { - return fromEigen(Eigen::Quaterniond(R)); + static SO3 from_mat(const Eigen::Matrix3d& R) { + return from_eigen(Eigen::Quaterniond(R)); } SO3 inverse() const { @@ -323,11 +323,11 @@ struct SO3 { } SO3 operator*(const SO3& other) const { - return fromEigen(toEigen() * other.toEigen()); + return from_eigen(to_eigen() * other.to_eigen()); } Eigen::Vector3d rotate(const Eigen::Vector3d& v) const { - return toEigen() * v; + return to_eigen() * v; } static Eigen::Matrix3d hat(const Eigen::Vector3d& xi) { @@ -339,25 +339,25 @@ struct SO3 { static SO3 exp(const Eigen::Vector3d& v) { Eigen::AngleAxisd axis(v.norm(), v.normalized()); Eigen::Quaterniond q(axis); - return fromEigen(q); + return from_eigen(q); } Eigen::Vector3d log() const { - Eigen::Quaterniond q = toEigen(); + Eigen::Quaterniond q = to_eigen(); auto axis = Eigen::AngleAxisd(q); return axis.angle() * axis.axis(); } - Eigen::Matrix3d toMat() const { - return toEigen().toRotationMatrix(); + Eigen::Matrix3d to_mat() const { + return to_eigen().toRotationMatrix(); } - std::string toString() const { + std::string to_string() const { return "SO3(x: " + std::to_string(qx) + ", y: " + std::to_string(qy) + ", z: " + std::to_string(qz) + ", w: " + std::to_string(qw) + ")"; } - std::string toStringBrief() const { + std::string to_string_brief() const { return "x: " + std::to_string(qx) + ", y: " + std::to_string(qy) + ", z: " + std::to_string(qz) + ", w: " + std::to_string(qw); } @@ -381,13 +381,13 @@ struct SE3 { return SE3(SO3::identity(), Eigen::Vector3d::Zero()); } - static SE3 fromMat(const Eigen::Matrix4d& T) { - return SE3(SO3::fromMat(T.block<3, 3>(0, 0)), T.block<3, 1>(0, 3)); + static SE3 from_mat(const Eigen::Matrix4d& T) { + return SE3(SO3::from_mat(T.block<3, 3>(0, 0)), T.block<3, 1>(0, 3)); } - Eigen::Matrix4d toMat() const { + Eigen::Matrix4d to_mat() const { Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - T.block<3, 3>(0, 0) = rot.toMat(); + T.block<3, 3>(0, 0) = rot.to_mat(); T.block<3, 1>(0, 3) = trans; return T; } @@ -451,9 +451,9 @@ struct SE3 { return SE3(rot * other.rot, rot.rotate(other.trans) + trans); } - std::string toString() const { + std::string to_string() const { std::ostringstream oss; - oss << "SE3(rot: [" << rot.toStringBrief() << "], " + oss << "SE3(rot: [" << rot.to_string_brief() << "], " << "t: [" << trans.transpose() << "])"; return oss.str(); } diff --git a/python/evalio/datasets/botanic_garden.py b/python/evalio/datasets/botanic_garden.py index e07278bc..f0ba2c8d 100644 --- a/python/evalio/datasets/botanic_garden.py +++ b/python/evalio/datasets/botanic_garden.py @@ -65,7 +65,7 @@ def ground_truth_raw(self) -> Trajectory: # ------------------------- For loading params ------------------------- # def imu_T_lidar(self) -> SE3: # https://github.com/robot-pesg/BotanicGarden/blob/main/calib/extrinsics/calib_chain.yaml - return SE3.fromMat( + return SE3.from_mat( np.array( [ [ diff --git a/python/evalio/datasets/cumulti.py b/python/evalio/datasets/cumulti.py index 76c8be91..33b49cf3 100644 --- a/python/evalio/datasets/cumulti.py +++ b/python/evalio/datasets/cumulti.py @@ -114,7 +114,7 @@ def ground_truth_raw(self) -> Trajectory: # ------------------------- For loading params ------------------------- # def imu_T_lidar(self) -> SE3: # Supplied by CU-Multi Authors - return SE3.fromMat( + return SE3.from_mat( np.array( [ [-1.0, 0.0, 0.0, -0.058038], diff --git a/python/evalio/datasets/helipr.py b/python/evalio/datasets/helipr.py index 6c4b9044..ef87c70f 100644 --- a/python/evalio/datasets/helipr.py +++ b/python/evalio/datasets/helipr.py @@ -86,7 +86,7 @@ def ground_truth_raw(self) -> Trajectory: # ------------------------- For loading params ------------------------- # def imu_T_lidar(self) -> SE3: return SE3( - SO3.fromMat( + SO3.from_mat( np.array( [ [0.999715495593027, 0.0223448061210468, -0.00834490926264448], diff --git a/python/evalio/datasets/multi_campus.py b/python/evalio/datasets/multi_campus.py index cfebf082..97cf11a7 100644 --- a/python/evalio/datasets/multi_campus.py +++ b/python/evalio/datasets/multi_campus.py @@ -88,7 +88,7 @@ def imu_T_lidar(self) -> SE3: # The NTU sequences use the ATV platform # Taken from calib file at: https://mcdviral.github.io/Download.html#calibration if "ntu" in self.seq_name: - return SE3.fromMat( + return SE3.from_mat( np.array( [ [ @@ -116,7 +116,7 @@ def imu_T_lidar(self) -> SE3: # The KTH and TUHH sequences use the hand-held platform # Taken from calib file at: https://mcdviral.github.io/Download.html#calibration elif "kth" in self.seq_name or "tuhh" in self.seq_name: - return SE3.fromMat( + return SE3.from_mat( np.array( [ [ diff --git a/tests/test_se3.py b/tests/test_se3.py index 9de5dfc3..4c500ab1 100644 --- a/tests/test_se3.py +++ b/tests/test_se3.py @@ -39,7 +39,7 @@ def test_from_matrix(): mat[:3, :3] = r.as_matrix() mat[:3, 3] = t - se3 = SE3.fromMat(mat) + se3 = SE3.from_mat(mat) is_close(T(r, t), se3) @@ -56,7 +56,7 @@ def test_to_matrix(): mat[:3, :3] = r.as_matrix() mat[:3, 3] = t - assert np.allclose(mat, se3.toMat()) + assert np.allclose(mat, se3.to_mat()) def test_log_exp(): diff --git a/tests/test_so3.py b/tests/test_so3.py index 5382cbd6..a2510a82 100644 --- a/tests/test_so3.py +++ b/tests/test_so3.py @@ -22,7 +22,7 @@ def test_constructor(): def test_from_matrix(): r = R.from_rotvec([0.1, 0.2, 0.3]) - so3 = SO3.fromMat(r.as_matrix()) + so3 = SO3.from_mat(r.as_matrix()) is_close(r, so3)