Merge branch 'master' of github.com:KlemenDEV/VisionDrone

KlemenDEV · May 17, 2021 · 9f75c76 · 9f75c76
2 parents 7cde309 + b63525f
commit 9f75c76
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 15 deletions.
diff --git 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
 

diff --git 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
@@ -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"