Manif refactor

.gitmodules

@@ -8,3 +8,7 @@
 	url = https://dawn.googlesource.com/dawn
 	shallow = true
 	branch = chromium/6108
+[submodule "deps/manif"]
+	path = deps/manif
+	url = https://github.com/artivis/manif.git
+	shallow = true

CMakeLists.txt

@@ -91,12 +91,15 @@ if (MROVER_BUILD_SIM)
     find_package(glfw3 REQUIRED)
 
     include(cmake/webgpu.cmake)
-
-    add_subdirectory(deps/glfw3webgpu)
+    add_subdirectory(deps/glfw3webgpu SYSTEM)
+    find_package(Assimp REQUIRED)
+    find_package(Bullet REQUIRED)
+    find_package(glfw3 REQUIRED)
 endif ()
 find_package(OpenCV REQUIRED)
 find_package(ZED QUIET)
 find_package(Eigen3 REQUIRED)
+add_subdirectory(deps/manif SYSTEM)
 find_package(PkgConfig REQUIRED)
 pkg_search_module(NetLink libnl-3.0)
 pkg_search_module(NetLinkRoute libnl-route-3.0)
@@ -146,6 +149,7 @@ catkin_package()
 ## Libraries
 
 mrover_add_library(lie src/util/lie/*.cpp src/util/lie)
+target_link_libraries(lie PUBLIC manif)
 
 ## ESW
 
@@ -187,7 +191,7 @@ endif ()
 ## Perception
 
 mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp)
-mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie)
+mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif)
 
 if (CUDA_FOUND)
     mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming)
@@ -200,7 +204,7 @@ endif ()
 if (ZED_FOUND)
     mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp)
     mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})
-    mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie)
+    mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie manif)
     mrover_nodelet_defines(zed
             ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode
             __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore
@@ -216,7 +220,7 @@ mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp)
 if (MROVER_BUILD_SIM)
     mrover_add_nodelet(simulator src/simulator/*.cpp src/simulator src/simulator/pch.hpp)
     mrover_nodelet_include_directories(simulator ${ASSIMP_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} src/simulator/imgui)
-    mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie)
+    mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie manif)
     mrover_nodelet_defines(simulator BOOST_THREAD_PROVIDES_FUTURE)
 endif ()
 

package.xml

@@ -67,6 +67,7 @@
   <!-- Localization -->
   <depend>rviz_imu_plugin</depend>
   <depend>robot_localization</depend>
+  <depend>rtcm_msgs</depend>
 
   <!-- Navigation -->
 

pyproject.toml

@@ -23,6 +23,7 @@ dependencies = [
     "moteus==0.3.59",
     "pymap3d==3.0.1",
     "aenum==3.1.15",
+    "pyubx2==1.2.35",
     "daphne==4.0.0",
     "channels==4.0.0",
 ]

src/perception/tag_detector/pch.hpp

@@ -30,5 +30,6 @@
 #include <mrover/DetectorParamsConfig.h>
 
 #include <loop_profiler.hpp>
+#include <manif/manif.h>
 #include <point.hpp>
 #include <se3.hpp>

src/perception/tag_detector/tag_detector.cpp

@@ -140,5 +140,5 @@ namespace mrover {
         res.success = true;
         return true;
     }
-
+    
 } // namespace mrover

src/perception/tag_detector/tag_detector.hpp

@@ -6,7 +6,7 @@ namespace mrover {
         int id = -1;
         int hitCount = 0;
         cv::Point2f imageCenter{};
-        std::optional<SE3> tagInCam;
+        std::optional<SE3d> tagInCam;
     };
 
     class TagDetectorNodelet : public nodelet::Nodelet {
@@ -53,7 +53,7 @@ namespace mrover {
 
         void publishThresholdedImage();
 
-        std::optional<SE3> getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v);
+        std::optional<SE3d> getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v);
 
     public:
         TagDetectorNodelet() = default;

src/perception/tag_detector/tag_detector.processing.cpp

@@ -8,7 +8,7 @@ namespace mrover {
      * @param u     X Pixel Position
      * @param v     Y Pixel Position
      */
-    std::optional<SE3> TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) {
+    std::optional<SE3d> TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) {
         assert(cloudPtr);
 
         if (u >= cloudPtr->width || v >= cloudPtr->height) {
@@ -23,7 +23,7 @@ namespace mrover {
             return std::nullopt;
         }
 
-        return std::make_optional<SE3>(R3{point.x, point.y, point.z}, SO3{});
+        return std::make_optional<SE3d>(R3{point.x, point.y, point.z}, SO3d::Identity());
     }
 
     /**
@@ -81,7 +81,7 @@ namespace mrover {
             if (tag.tagInCam) {
                 // Publish tag to immediate
                 std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id);
-                SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value());
+                SE3Conversions::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value());
             }
         }
 
@@ -108,8 +108,8 @@ namespace mrover {
                     std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id);
                     // Publish tag to odom
                     std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId;
-                    SE3 tagInParent = SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId);
-                    SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent);
+                    SE3d tagInParent = SE3Conversions::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId);
+                    SE3Conversions::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent);
                 } catch (tf2::ExtrapolationException const&) {
                     NODELET_WARN("Old data for immediate tag");
                 } catch (tf2::LookupException const&) {

src/perception/zed_wrapper/pch.hpp

@@ -24,6 +24,5 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <se3.hpp>
-#include <point.hpp>
 #include <loop_profiler.hpp>
+#include <point.hpp>

src/perception/zed_wrapper/zed_wrapper.bridge.cu

@@ -1,5 +1,4 @@
 // Be careful what you include in this file, it is compiled with nvcc (NVIDIA CUDA compiler)
-// For example OpenCV and lie includes cause problems
 
 #include "zed_wrapper.hpp"
 

src/perception/zed_wrapper/zed_wrapper.cpp

@@ -1,5 +1,8 @@
 #include "zed_wrapper.hpp"
 
+#include <manif/manif.h>
+#include <se3.hpp>
+
 namespace mrover {
 
     /**
@@ -252,11 +255,11 @@ namespace mrover {
                         sl::Translation const& translation = pose.getTranslation();
                         sl::Orientation const& orientation = pose.getOrientation();
                         try {
-                            SE3 leftCameraInOdom{{translation.x, translation.y, translation.z},
-                                                 Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()};
-                            SE3 leftCameraInBaseLink = SE3::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame");
-                            SE3 baseLinkInOdom = leftCameraInBaseLink * leftCameraInOdom;
-                            SE3::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom);
+                            SE3d leftCameraInOdom{{translation.x, translation.y, translation.z},
+                                                  Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()};
+                            SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame");
+                            SE3d baseLinkInOdom = leftCameraInOdom * baseLinkToLeftCamera;
+                            SE3Conversions::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom);
                         } catch (tf2::TransformException& e) {
                             NODELET_WARN_STREAM("Failed to get transform: " << e.what());
                         }

src/simulator/pch.hpp

@@ -68,6 +68,7 @@
 
 #include <bimap.hpp>
 #include <loop_profiler.hpp>
+#include <manif/manif.h>
 #include <params_utils.hpp>
 #include <point.hpp>
 #include <se3.hpp>

src/simulator/simulator.controls.cpp

@@ -83,23 +83,32 @@ namespace mrover {
 
     auto SimulatorNodelet::freeLook(Clock::duration dt) -> void {
         float flySpeed = mFlySpeed * std::chrono::duration_cast<std::chrono::duration<float>>(dt).count();
-        if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) {
-            mCameraInWorld = SE3{R3{0, -flySpeed, 0}, SO3{}} * mCameraInWorld;
-        }
-        if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) {
-            mCameraInWorld = SE3{R3{0, flySpeed, 0}, SO3{}} * mCameraInWorld;
-        }
+        SE3d::Tangent delta_x, delta_y, delta_z;
+        delta_x << Eigen::Vector3d::UnitX(), Eigen::Vector3d::Zero();
+        delta_y << Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero();
+        delta_z << Eigen::Vector3d::UnitZ(), Eigen::Vector3d::Zero();
+
+        // TODO: turn this into a single rplus and lplus operation
+        // overloaded rplus for movement relative to camera frame
         if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) {
-            mCameraInWorld = SE3{R3{flySpeed, 0, 0}, SO3{}} * mCameraInWorld;
+            mCameraInWorld += delta_x * flySpeed;
         }
         if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) {
-            mCameraInWorld = SE3{R3{-flySpeed, 0, 0}, SO3{}} * mCameraInWorld;
+            mCameraInWorld += delta_x * -flySpeed;
         }
+        if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) {
+            mCameraInWorld += delta_y * flySpeed;
+        }
+        if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) {
+            mCameraInWorld += delta_y * -flySpeed;
+        }
+
+        // overloaded lplus for movement relative to world frame
         if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) {
-            mCameraInWorld = mCameraInWorld * SE3{R3{0, 0, flySpeed}, SO3{}};
+            mCameraInWorld = delta_z * flySpeed + mCameraInWorld;
         }
         if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) {
-            mCameraInWorld = mCameraInWorld * SE3{R3{0, 0, -flySpeed}, SO3{}};
+            mCameraInWorld = delta_z * -flySpeed + mCameraInWorld;
         }
 
         Eigen::Vector2i size;
@@ -112,10 +121,10 @@ namespace mrover {
 
         Eigen::Vector2d delta = (mouse - center) * mLookSense;
 
-        // TODO(quintin): use lie algebra more here? we have a perturbation in the tangent space
-        R3 p = mCameraInWorld.position();
-        SO3 q = SO3{delta.y(), R3::UnitY()} * mCameraInWorld.rotation() * SO3{-delta.x(), R3::UnitZ()};
-        mCameraInWorld = SE3{p, q};
+        // lplus for tilt in camera frame, rplus for pan in world frame
+        SO3d::Tangent delta_Ry = Eigen::Vector3d::UnitY() * delta.y();
+        SO3d::Tangent delta_Rz = Eigen::Vector3d::UnitZ() * -delta.x();
+        mCameraInWorld.asSO3() = delta_Rz + mCameraInWorld.asSO3() + delta_Ry;
 
         centerCursor();
     }

src/simulator/simulator.gui.cpp

@@ -94,15 +94,15 @@ namespace mrover {
                 URDF const& rover = it->second;
 
                 {
-                    SE3 baseLinkInMap = rover.linkInWorld("base_link");
-                    R3 p = baseLinkInMap.position();
-                    S3 q = baseLinkInMap.rotation().quaternion();
+                    SE3d baseLinkInMap = rover.linkInWorld("base_link");
+                    R3 p = baseLinkInMap.translation();
+                    S3 q = baseLinkInMap.quat();
                     ImGui::Text("Rover Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z());
                     ImGui::Text("Rover Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z());
                 }
                 {
-                    R3 p = mCameraInWorld.position();
-                    S3 q = mCameraInWorld.rotation().quaternion();
+                    R3 p = mCameraInWorld.translation();
+                    S3 q = mCameraInWorld.quat();
                     ImGui::Text("Camera Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z());
                     ImGui::Text("Camera Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z());
                 }
@@ -134,6 +134,13 @@ namespace mrover {
                 }
             }
 
+            {
+                R3 rayStart = mCameraInWorld.translation();
+                R3 rayEnd = rayStart + mCameraInWorld.rotation().matrix().col(0);
+                // btMul
+                // mDynamicsWorld->rayTest(r3ToBtVector3(rayStart), r3ToBtVector3(rayEnd), [
+            }
+
             for (Camera const& camera: mCameras) {
                 float aspect = static_cast<float>(camera.resolution.x()) / static_cast<float>(camera.resolution.y());
                 ImGui::Image(camera.colorTextureView, {320, 320 / aspect}, {0, 0}, {1, 1});

src/simulator/simulator.hpp

@@ -4,6 +4,7 @@
 
 #include "glfw_pointer.hpp"
 #include "wgpu_objects.hpp"
+#include "glfw_pointer.hpp"
 
 using namespace std::literals;
 
@@ -97,7 +98,7 @@ namespace mrover {
 
         auto makeCameraForLink(SimulatorNodelet& simulator, btMultibodyLink const* link) -> Camera;
 
-        [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3;
+        [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3d;
     };
 
     struct PeriodicTask {
@@ -305,7 +306,7 @@ namespace mrover {
 
         auto getUrdf(std::string const& name) -> std::optional<std::reference_wrapper<URDF>>;
 
-        SE3 mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3{}};
+        SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d::Identity()};
 
         std::vector<StereoCamera> mStereoCameras;
         std::vector<Camera> mCameras;
@@ -432,7 +433,7 @@ namespace mrover {
 
     auto urdfPoseToBtTransform(urdf::Pose const& pose) -> btTransform;
 
-    auto btTransformToSe3(btTransform const& transform) -> SE3;
+    auto btTransformToSe3(btTransform const& transform) -> SE3d;
 
     auto computeCameraToClip(float fovY, float aspect, float zNear, float zFar) -> Eigen::Matrix4f;
 

src/simulator/simulator.physics.cpp

@@ -2,10 +2,12 @@
 
 namespace mrover {
 
-    auto btTransformToSe3(btTransform const& transform) -> SE3 {
+    auto btTransformToSe3(btTransform const& transform) -> SE3d {
         btVector3 const& p = transform.getOrigin();
         btQuaternion const& q = transform.getRotation();
-        return SE3{R3{p.x(), p.y(), p.z()}, SO3{q.w(), q.x(), q.y(), q.z()}};
+        // Note: Must convert the Bullet quaternion (floats) to a normalized Eigen quaternion (doubles).
+        //       Otherwise the normality check will fail in the SO3 constructor.
+        return SE3d{R3{p.x(), p.y(), p.z()}, Eigen::Quaterniond{q.w(), q.x(), q.y(), q.z()}.normalized()};
     }
 
     auto SimulatorNodelet::initPhysics() -> void {
@@ -76,9 +78,9 @@ namespace mrover {
                     int index = urdf.linkNameToMeta.at(link->name).index;
                     // TODO(quintin): figure out why we need to negate rvector
                     btTransform parentToChild{urdf.physics->getParentToLocalRot(index), -urdf.physics->getRVector(index)};
-                    SE3 childInParent = btTransformToSe3(parentToChild.inverse());
+                    SE3d childInParent = btTransformToSe3(parentToChild.inverse());
 
-                    SE3::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent);
+                    SE3Conversions::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent);
                 }
 
                 for (urdf::JointSharedPtr const& child_joint: link->child_joints) {
@@ -95,17 +97,17 @@ namespace mrover {
                 if (auto modelOpt = getUrdf(modelName)) {
                     URDF const& model = *modelOpt;
 
-                    SE3 modelInMap = btTransformToSe3(model.physics->getBaseWorldTransform());
-                    SE3 roverInMap = btTransformToSe3(rover.physics->getBaseWorldTransform());
+                    SE3d modelInMap = btTransformToSe3(model.physics->getBaseWorldTransform());
+                    SE3d roverInMap = btTransformToSe3(rover.physics->getBaseWorldTransform());
 
-                    R3 roverToModel = modelInMap.position() - roverInMap.position();
+                    R3 roverToModel = modelInMap.translation() - roverInMap.translation();
                     double roverDistanceToModel = roverToModel.norm();
                     roverToModel /= roverDistanceToModel;
                     R3 roverForward = roverInMap.rotation().matrix().col(0);
                     double roverDotModel = roverToModel.dot(roverForward);
 
                     if (roverDotModel > 0 && roverDistanceToModel < threshold) {
-                        SE3::pushToTfTree(mTfBroadcaster, modelName, "map", modelInMap);
+                        SE3Conversions::pushToTfTree(mTfBroadcaster, modelName, "map", modelInMap);
                     }
                 }
             };
@@ -115,7 +117,7 @@ namespace mrover {
         }
     }
 
-    auto URDF::linkInWorld(std::string const& linkName) const -> SE3 {
+    auto URDF::linkInWorld(std::string const& linkName) const -> SE3d {
         int index = linkNameToMeta.at(linkName).index;
         return btTransformToSe3(index == -1 ? physics->getBaseWorldTransform() : physics->getLink(index).m_cachedWorldTransform);
     }