version 1.1.2

README.md

@@ -57,8 +57,8 @@ You will need to install the following dependencies,
 
 then download and expand the archive into your catkin workspace:
 
-    wget https://www.doc.ic.ac.uk/~sleutene/software/okvis_ros-1.1.1.zip
-    unzip okvis_ros-1.1.1.zip && rm okvis_ros-1.1.1.zip
+    wget https://www.doc.ic.ac.uk/~sleutene/software/okvis_ros-1.1.2.zip
+    unzip okvis_ros-1.1.2.zip && rm okvis_ros-1.1.2.zip
 
 Or, if you were given bitbucket access, clone the repository into your catkin
 workspace:

config/rviz.rviz

@@ -3,10 +3,9 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /Global Options1
-      Splitter Ratio: 0.5
-    Tree Height: 425
+      Expanded: ~
+      Splitter Ratio: 0.784387
+    Tree Height: 454
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -26,7 +25,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Matched Landmarks
+    SyncSource: ""
   - Class: rviz/Help
     Name: Help
 Visualization Manager:
@@ -180,20 +179,20 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 703
+  Height: 732
   Help:
     collapsed: false
   Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d400000238fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000238000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000800480065006c007000000002ed000000b60000007600ffffff000000010000010f00000238fc0200000003fb0000000a00560069006500770073010000002800000238000000b000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006150000003bfc0100000002fb0000000800540069006d0065010000000000000615000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003260000023800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000010f00000255fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000255000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000800480065006c007000000002ed000000b60000007600ffffff000000010000010f00000255fc0200000003fb0000000a00560069006500770073000000002800000255000000b000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004870000003bfc0100000002fb0000000800540069006d0065010000000000000487000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003720000025500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 1557
-  X: 2100
-  Y: 161
+    collapsed: true
+  Width: 1159
+  X: 94
+  Y: 49

include/okvis/Publisher.hpp

@@ -273,6 +273,7 @@ class Publisher
   ros::Publisher pubObometry_;  ///< The publisher for the odometry.
   ros::Publisher pubPath_;  ///< The publisher for the path.
   ros::Publisher pubTransform_; ///< The publisher for the transform.
+  ros::Publisher pubMesh_; ///< The publisher for a robot / camera mesh.
   std::vector<image_transport::Publisher> pubImagesVector_; ///< The publisher for the images.
   std::vector<image_transport::ImageTransport> imageTransportVector_; ///< The image transporters.
 
@@ -289,6 +290,7 @@ class Publisher
   pcl::PointCloud<pcl::PointXYZRGB> pointsTransferred_; ///< Point cloud for transferred/marginalised points.
   std::vector<cv::Mat> images_; ///< The images.
   nav_msgs::Path path_; ///< The path message.
+  visualization_msgs::Marker meshMsg_; ///< Mesh message.
 
   /// @}
 

launch/okvis_node.launch

@@ -1,6 +1,7 @@
 <launch>
   <node name="okvis_node" pkg="okvis_ros" type="okvis_node" output="screen">
     <param name="config_filename" value="$(find okvis_ros)/okvis/config/config_DOC1357.yaml" />
+    <param name="mesh_file" value="firefly.dae" />
     <remap from="/camera0" to="/cam0/image_raw" />
     <remap from="/camera1" to="/cam1/image_raw" />
     <remap from="/calibration0" to="/cam0/calibration" />

launch/okvis_node_synchronous.launch

@@ -1,5 +1,6 @@
 <launch>
   <node name="okvis_node" pkg="okvis_ros" type="okvis_node_synchronous" output="screen" args="$(arg config) $(arg bag)">
+    <param name="mesh_file" value="firefly.dae" />
     <param name="camera_topic_0" value="/cam0/image_raw" />
     <param name="camera_topic_1" value="/cam1/image_raw" />
     <param name="imu_topic" value="/imu0" />

src/Publisher.cpp

@@ -102,6 +102,15 @@ void Publisher::setNodeHandle(ros::NodeHandle& nh)
   pubPath_ = nh_->advertise<nav_msgs::Path>("okvis_path", 1);
   pubTransform_ = nh_->advertise<geometry_msgs::TransformStamped>(
       "okvis_transform", 1);
+  pubMesh_ = nh_->advertise<visualization_msgs::Marker>( "okvis_mesh", 0 );
+  // where to get the mesh from
+  std::string mesh_file;
+  if (nh_->getParam("mesh_file", mesh_file)) {
+    meshMsg_.mesh_resource = "package://okvis_ros/meshes/"+mesh_file;
+  } else {
+    LOG(INFO) << "no mesh found for visualisation, set ros param mesh_file, if desired";
+    meshMsg_.mesh_resource = "";
+  }
 }
 
 // Write CSV header.
@@ -221,6 +230,44 @@ void Publisher::setPose(const okvis::kinematics::Transformation& T_WS)
   poseMsg_.transform.translation.y = r[1];
   poseMsg_.transform.translation.z = r[2];
 
+  // also do the mesh
+  /*if (parameters_.publishing.trackedBodyFrame == FrameName::S) {
+    meshMsg_.child_frame_id = "sensor";
+  } else if (parameters_.publishing.trackedBodyFrame == FrameName::B) {
+    meshMsg_.child_frame_id = "body";
+  } else {
+    meshMsg_.child_frame_id = "body";
+  }*/
+  meshMsg_.header.frame_id = "world";
+  meshMsg_.header.stamp = _t;
+  meshMsg_.type = visualization_msgs::Marker::MESH_RESOURCE;
+  if ((ros::Time::now() - _t).toSec() > 10.0)
+    meshMsg_.header.stamp = ros::Time::now();
+
+  // fill orientation
+  meshMsg_.pose.orientation.x = q.x();
+  meshMsg_.pose.orientation.y = q.y();
+  meshMsg_.pose.orientation.z = q.z();
+  meshMsg_.pose.orientation.w = q.w();
+
+  // fill position
+  meshMsg_.pose.position.x = r[0];
+  meshMsg_.pose.position.y = r[1];
+  meshMsg_.pose.position.z = r[2];
+
+  // scale -- needed
+  meshMsg_.scale.x = 1.0;
+  meshMsg_.scale.y = 1.0;
+  meshMsg_.scale.z = 1.0;
+
+	meshMsg_.action = visualization_msgs::Marker::ADD;
+	meshMsg_.color.a = 1.0; // Don't forget to set the alpha!
+	meshMsg_.color.r = 1.0;
+	meshMsg_.color.g = 1.0;
+	meshMsg_.color.b = 1.0;
+
+  // embedded material / colour
+  //meshMsg_.mesh_use_embedded_materials = true;
 }
 
 // Set the odometry message that is published next.
@@ -413,6 +460,8 @@ void Publisher::publishPose()
   if ((_t - lastOdometryTime2_).toSec() < 1.0 / parameters_.publishing.publishRate)
     return;  // control the publish rate
   pubTf_.sendTransform(poseMsg_);
+  if(!meshMsg_.mesh_resource.empty())
+    pubMesh_.publish(meshMsg_);  //publish stamped mesh
   lastOdometryTime2_ = _t;  // remember
 }
 
@@ -422,6 +471,8 @@ void Publisher::publishOdometry()
   if ((_t - lastOdometryTime_).toSec() < 1.0 / parameters_.publishing.publishRate)
     return;  // control the publish rate
   pubObometry_.publish(odometryMsg_);
+  if(!meshMsg_.mesh_resource.empty())
+    pubMesh_.publish(meshMsg_);  //publish stamped mesh
   lastOdometryTime_ = _t;  // remember
 }