Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions cpp/bindings/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
2 changes: 1 addition & 1 deletion cpp/bindings/pipeline.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_<evalio::Pipeline, PyPipeline>(m, "Pipeline")
.def(nb::init<>(), "Construct a new pipeline.")
.def_static("name", &evalio::Pipeline::name, "Name of the pipeline.")
Expand Down
2 changes: 1 addition & 1 deletion cpp/bindings/pipelines/bindings.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_<KissICP, evalio::Pipeline>(m, "KissICP")
Expand Down
2 changes: 1 addition & 1 deletion cpp/bindings/pipelines/ct_icp.h
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion cpp/bindings/pipelines/lio_sam.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion cpp/bindings/pipelines/loam.h
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion cpp/bindings/ros_pc2.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_<DataType>(m, "DataType")
.value("UINT8", DataType::UINT8)
.value("INT8", DataType::INT8)
Expand Down
32 changes: 16 additions & 16 deletions cpp/bindings/types.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_<VisOption>(m, "VisOption", nb::is_flag())
.value("MAP", VisOption::MAP, "Visualize the map.")
.value("FEATURES", VisOption::FEATURES, "Visualize the features.")
Expand Down Expand Up @@ -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__",
Expand Down Expand Up @@ -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__",
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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.";
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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_<SO3>(m, "SO3")
Expand All @@ -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(
Expand All @@ -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__",
Expand Down Expand Up @@ -551,14 +551,14 @@ inline void makeTypes(nb::module_& m) {
.def(nb::init<SE3>(), "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",
Expand Down Expand Up @@ -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__",
Expand Down
2 changes: 1 addition & 1 deletion cpp/evalio/convert/sophus.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
62 changes: 31 additions & 31 deletions cpp/evalio/types.h
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}

Expand Down Expand Up @@ -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 =
Expand Down Expand Up @@ -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())
Expand All @@ -173,9 +173,9 @@ struct LidarMeasurement {
LidarMeasurement(Stamp stamp, std::vector<Point> 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();
}
Expand Down Expand Up @@ -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)
Expand All @@ -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();
Expand Down Expand Up @@ -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
Expand All @@ -302,32 +302,32 @@ 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()};
}

static SO3 identity() {
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 {
return SO3 {.qx = -qx, .qy = -qy, .qz = -qz, .qw = qw};
}

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) {
Expand All @@ -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);
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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();
}
Expand Down
2 changes: 1 addition & 1 deletion python/evalio/datasets/botanic_garden.py
Original file line number Diff line number Diff line change
Expand Up @@ -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(
[
[
Expand Down
Loading