From 461c62a53b958555e8ae7833649b6108cc69af79 Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin Date: Wed, 24 Jan 2024 16:45:35 +0100 Subject: [PATCH 1/2] Transform many points --- pycolmap/geometry/bindings.h | 45 ++++++++++++++++++++++++++------- pycolmap/scene/image.h | 48 ------------------------------------ 2 files changed, 36 insertions(+), 57 deletions(-) diff --git a/pycolmap/geometry/bindings.h b/pycolmap/geometry/bindings.h index 117f028..98b5739 100644 --- a/pycolmap/geometry/bindings.h +++ b/pycolmap/geometry/bindings.h @@ -22,15 +22,27 @@ void BindGeometry(py::module& m) { py::class_ PyRotation3d(m, "Rotation3d"); PyRotation3d.def(py::init([]() { return Eigen::Quaterniond::Identity(); })) - .def(py::init(), "xyzw"_a) - .def(py::init(), "rotmat"_a) + .def(py::init(), + "xyzw"_a, + "Quaternion in [x,y,z,w] format.") + .def(py::init(), + "rotmat"_a, + "3x3 rotation matrix.") + .def_property( + "quat", + py::overload_cast<>(&Eigen::Quaterniond::coeffs), + [](Eigen::Quaterniond& self, const Eigen::Vector4d& quat) { + self.coeffs() = quat; + }, + "Quaternion in [x,y,z,w] format.") .def(py::self * Eigen::Quaterniond()) .def(py::self * Eigen::Vector3d()) - .def_property("quat", - py::overload_cast<>(&Eigen::Quaterniond::coeffs), - [](Eigen::Quaterniond& self, const Eigen::Vector4d& quat) { - self.coeffs() = quat; - }) + .def("__mul__", + [](const Eigen::Quaterniond& self, + const py::EigenDRef& points) + -> Eigen::MatrixX3d { + return points * self.toRotationMatrix().transpose(); + }) .def("normalize", &Eigen::Quaterniond::normalize) .def("matrix", &Eigen::Quaterniond::toRotationMatrix) .def("norm", &Eigen::Quaterniond::norm) @@ -53,8 +65,15 @@ void BindGeometry(py::module& m) { .def_readwrite("translation", &Rigid3d::translation) .def("matrix", &Rigid3d::ToMatrix) .def("essential_matrix", &EssentialMatrixFromPose) - .def(py::self * Eigen::Vector3d()) .def(py::self * Rigid3d()) + .def(py::self * Eigen::Vector3d()) + .def("__mul__", + [](const Rigid3d& t, const py::EigenDRef& points) + -> Eigen::MatrixX3d { + return (points * t.rotation.toRotationMatrix().transpose()) + .rowwise() + + t.translation.transpose(); + }) .def("inverse", static_cast(&Inverse)) .def_static("interpolate", &InterpolateCameraPoses) .def("__repr__", [](const Rigid3d& self) { @@ -76,8 +95,16 @@ void BindGeometry(py::module& m) { .def_readwrite("rotation", &Sim3d::rotation) .def_readwrite("translation", &Sim3d::translation) .def("matrix", &Sim3d::ToMatrix) - .def(py::self * Eigen::Vector3d()) .def(py::self * Sim3d()) + .def(py::self * Eigen::Vector3d()) + .def("__mul__", + [](const Sim3d& t, const py::EigenDRef& points) + -> Eigen::MatrixX3d { + return (t.scale * + (points * t.rotation.toRotationMatrix().transpose())) + .rowwise() + + t.translation.transpose(); + }) .def("transform_camera_world", &TransformCameraWorld) .def("inverse", static_cast(&Inverse)) .def("__repr__", [](const Sim3d& self) { diff --git a/pycolmap/scene/image.h b/pycolmap/scene/image.h index 93a3d69..68ece34 100644 --- a/pycolmap/scene/image.h +++ b/pycolmap/scene/image.h @@ -241,54 +241,6 @@ void BindImage(py::module& m) { return valid_points2D; }) - .def( - "project", - [](const Image& self, - const std::vector& world_coords) { - std::vector image_points(world_coords.size()); - for (int idx = 0; idx < world_coords.size(); ++idx) { - image_points[idx] = - (self.CamFromWorld() * world_coords[idx]).hnormalized(); - } - return image_points; - }, - "Project list of world points to image (xy).") - .def( - "project", - [](const Image& self, const std::vector& point3Ds) { - std::vector world_points(point3Ds.size()); - for (int idx = 0; idx < point3Ds.size(); ++idx) { - world_points[idx] = - (self.CamFromWorld() * point3Ds[idx].xyz).hnormalized(); - } - return world_points; - }, - "Project list of point3Ds to image (xy).") - .def( - "transform_to_image", - [](const Image& self, - const std::vector& world_coords) { - std::vector image_points(world_coords.size()); - for (int idx = 0; idx < world_coords.size(); ++idx) { - image_points[idx] = (self.CamFromWorld() * world_coords[idx]); - } - return image_points; - }, - "Project list of points in world coordinate frame to image " - "coordinates (xyz).") - .def( - "transform_to_world", - [](const Image& self, - const std::vector& image_coords) { - std::vector world_points(image_coords.size()); - for (int idx = 0; idx < image_coords.size(); ++idx) { - world_points[idx] = - (Inverse(self.CamFromWorld()) * image_coords[idx]); - } - return world_points; - }, - "Project list of image points (with depth) to world coordinate " - "frame.") .def("__repr__", &PrintImage); MakeDataclass(PyImage); } From 30ac033e25ddef01e5d3775209da2daafa9ffe3c Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin Date: Wed, 24 Jan 2024 17:00:41 +0100 Subject: [PATCH 2/2] Fix --- pycolmap/geometry/bindings.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/pycolmap/geometry/bindings.h b/pycolmap/geometry/bindings.h index 98b5739..85f3d18 100644 --- a/pycolmap/geometry/bindings.h +++ b/pycolmap/geometry/bindings.h @@ -39,7 +39,7 @@ void BindGeometry(py::module& m) { .def(py::self * Eigen::Vector3d()) .def("__mul__", [](const Eigen::Quaterniond& self, - const py::EigenDRef& points) + const py::EigenDRef& points) -> Eigen::MatrixX3d { return points * self.toRotationMatrix().transpose(); }) @@ -68,7 +68,8 @@ void BindGeometry(py::module& m) { .def(py::self * Rigid3d()) .def(py::self * Eigen::Vector3d()) .def("__mul__", - [](const Rigid3d& t, const py::EigenDRef& points) + [](const Rigid3d& t, + const py::EigenDRef& points) -> Eigen::MatrixX3d { return (points * t.rotation.toRotationMatrix().transpose()) .rowwise() + @@ -98,7 +99,8 @@ void BindGeometry(py::module& m) { .def(py::self * Sim3d()) .def(py::self * Eigen::Vector3d()) .def("__mul__", - [](const Sim3d& t, const py::EigenDRef& points) + [](const Sim3d& t, + const py::EigenDRef& points) -> Eigen::MatrixX3d { return (t.scale * (points * t.rotation.toRotationMatrix().transpose()))