solved setPose for real now

felipetrentin · May 7, 2022 · 6b9579e · 6b9579e
1 parent 9ba8793
commit 6b9579e
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 7 deletions.
diff --git a/lib/src/main/cpp/t265wrapper.hpp b/lib/src/main/cpp/t265wrapper.hpp
@@ -15,7 +15,7 @@
 // <https://www.gnu.org/licenses/>.
 
 #include <jni.h>
-#include <librealsense2/rs.hpp>
+#include "librealsense2/rs.hpp"
 #include <mutex>
 /* Header for class com_spartronics4915_lib_T265Camera */
 

diff --git a/lib/src/main/java/com/spartronics4915/lib/T265Camera.java b/lib/src/main/java/com/spartronics4915/lib/T265Camera.java
@@ -334,11 +334,15 @@ public void sendOdometry(double velocityXMetersPerSecond, double velocityYMeters
      */
     public synchronized void setPose(Pose2d newPose) {
         synchronized (mUpdateMutex) {
+            Pose2d campose = (mLastRecievedCameraUpdate == null
+                    ? new Pose2d()
+                    : mLastRecievedCameraUpdate);
+
+            //camera offset, relative to arena
             mOriginOffset =
-                    newPose.relativeTo(
-                            mLastRecievedCameraUpdate == null
-                                    ? new Pose2d()
-                                    : mLastRecievedCameraUpdate);
+                    new Pose2d(
+                            newPose.getTranslation().minus(campose.getTranslation().rotateBy(campose.getRotation().times(-1).plus(newPose.getRotation()))),
+                            newPose.getRotation().minus(campose.getRotation()));
         }
     }
 
@@ -416,8 +420,7 @@ private synchronized void consumePoseUpdate(
         }
 
         final Pose2d transformedPose =
-                mOriginOffset.transformBy(
-                        new Transform2d(currentPose.getTranslation(), currentPose.getRotation()));
+                mOriginOffset.transformBy(new Transform2d(currentPose.getTranslation(), currentPose.getRotation()));
 
         mPoseConsumer.accept(
                 new CameraUpdate(transformedPose, new ChassisSpeeds(dx, dy, dtheta), confidence));