From 9a8dc7a773c18daf80c221d6ef38b97568a68717 Mon Sep 17 00:00:00 2001
From: KlemenDEV <klemen.pylo@gmail.com>
Date: Sun, 16 May 2021 12:22:10 +0200
Subject: [PATCH 1/2] orbslam proper position output

---
 src/orb_slam3/src/ros_mono.cc             | 21 ++++++++-------------
 src/runs/launch/bag_play_orb_slam3.launch |  2 +-
 2 files changed, 9 insertions(+), 14 deletions(-)

diff --git a/src/orb_slam3/src/ros_mono.cc b/src/orb_slam3/src/ros_mono.cc
index 885b0991..9472c7ea 100644
--- a/src/orb_slam3/src/ros_mono.cc
+++ b/src/orb_slam3/src/ros_mono.cc
@@ -57,7 +57,6 @@ int main(int argc, char **argv) {
 }
 
 void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &msg) {
-    // Copy the ros image message to cv::Mat.
     cv_bridge::CvImageConstPtr cv_ptr;
     try {
         cv_ptr = cv_bridge::toCvShare(msg);
@@ -70,32 +69,28 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &msg) {
     cv::Mat Tcw = mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());
 
     if (Tcw.rows == 4 && Tcw.cols == 4) { // valid data
-        std::cout << "data" << std::endl;
+        cv::Mat Twc(4, 4, CV_32F);
+        cv::invert(Tcw, Twc);
 
         nav_msgs::Odometry odom;
-
         geometry_msgs::PoseWithCovariance posevc;
-
         geometry_msgs::Pose pose;
-
         geometry_msgs::Point pt;
-        pt.x = Tcw.at<double>(0,3);
-        pt.y = Tcw.at<double>(1,3);
-        pt.z = Tcw.at<double>(2,3);
-
+        pt.x = Twc.at<float>(0, 3);
+        pt.y = Twc.at<float>(1, 3);
+        pt.z = Twc.at<float>(2, 3);
         pose.position = pt;
-
         posevc.pose = pose;
-
         odom.pose = posevc;
         odom.header.seq = 1;
         odom.header.stamp = ros::Time::now();
         odom.header.frame_id = "map";
         odom.child_frame_id = "map";
-
         point_pub.publish(odom);
 
-        std::cout << Tcw.at<double>(0,3) << " : " << Tcw.at<double>(1,3) << " : " << Tcw.at<double>(2,3) << std::endl;
+        std::cout << std::to_string(Twc.at<float>(0, 3)) << ", "
+                  << std::to_string(Twc.at<float>(1, 3)) << ", "
+                  << std::to_string(Twc.at<float>(2, 3)) << std::endl;
     }
 }
 
diff --git a/src/runs/launch/bag_play_orb_slam3.launch b/src/runs/launch/bag_play_orb_slam3.launch
index 57ccb99b..d51f1560 100644
--- a/src/runs/launch/bag_play_orb_slam3.launch
+++ b/src/runs/launch/bag_play_orb_slam3.launch
@@ -2,7 +2,7 @@
 
 <launch>
     <!-- PLAY ROSBAG -->
-    <node pkg="rosbag" type="play" name="data_loader" args="/home/pylo/Desktop/walk2.bag" required="true"/>
+    <node pkg="rosbag" type="play" name="data_loader" args="/home/pylo/Desktop/kalibr.bag" required="true"/>
 
     <node pkg="tf"
           type="static_transform_publisher"

From b63525f15e8ce8ced141b3b0e74af61b6737426f Mon Sep 17 00:00:00 2001
From: Klemen <klemen.pevec@pylo.co>
Date: Sun, 16 May 2021 14:10:45 +0200
Subject: [PATCH 2/2] Update BUILDING.md

---
 BUILDING.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/BUILDING.md b/BUILDING.md
index 001bdacf..ac80b0e6 100644
--- a/BUILDING.md
+++ b/BUILDING.md
@@ -104,7 +104,7 @@ sudo make install
 
 ## 5. Build the custom workspace
 
-Run catkin_make in the workspace
+Run `catkin_make` in the workspace. For release, use `catkin_make -DCMAKE_BUILD_TYPE=Release`
 
 # WiFi configuration