Patch for optical_flow.cpp to make its output conform to what px4flow module outputs

optical_flow.patch

diff --git a/clever/src/optical_flow.cpp b/clever/src/optical_flow.cpp
index 3856eb7..37b63c0 100644
--- a/clever/src/optical_flow.cpp
+++ b/clever/src/optical_flow.cpp
@@ -77,7 +77,7 @@ private:
 		flow_.integrated_ygyro = NAN;
 		flow_.integrated_zgyro = NAN;
 		flow_.time_delta_distance_us = 0;
-		flow_.distance = -1; // no distance sensor available
+		flow_.distance = 0; // no distance sensor available
 		flow_.temperature = 0;
 
 		ROS_INFO("Optical Flow initialized");
@@ -117,19 +117,19 @@ private:
 			img = img(cv::Rect((msg->width / 2 - roi_2_), (msg->height / 2 - roi_2_), roi_, roi_));
 		}
 
-		img.convertTo(curr_, CV_64F);
+		img.convertTo(curr_, CV_32F);
 
 		if (prev_.empty()) {
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
-			cv::createHanningWindow(hann_, curr_.size(), CV_64F);
+			cv::createHanningWindow(hann_, curr_.size(), CV_32F);
 
 		} else {
 			double response;
 			cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response);
 
 			// Publish raw shift in pixels
-			static geometry_msgs::Vector3Stamped shift_vec;
+			geometry_msgs::Vector3Stamped shift_vec;
 			shift_vec.header.stamp = msg->header.stamp;
 			shift_vec.header.frame_id = msg->header.frame_id;
 			shift_vec.vector.x = shift.x;
@@ -156,26 +156,27 @@ private:
 			double flow_y = atan2(points_undist[0].y, focal_length_y);
 
 			// // Convert to FCU frame
-			static geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
+			geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
 			flow_camera.header.frame_id = msg->header.frame_id;
 			flow_camera.header.stamp = msg->header.stamp;
-			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
-			flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
+			flow_camera.vector.x = flow_x; // +y means counter-clockwise rotation around Y axis
+			flow_camera.vector.y = flow_y; // +x means clockwise rotation around X axis
 			tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
 
 			// Calculate integration time
 			ros::Duration integration_time = msg->header.stamp - prev_stamp_;
-			uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
+			uint32_t integration_time_us = integration_time.toNSec() / 1000ull;
 
 			if (calc_flow_gyro_) {
 				try {
 					auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp);
-					static geometry_msgs::Vector3Stamped flow_gyro_fcu;
+					geometry_msgs::Vector3Stamped flow_gyro_fcu;
 					tf_buffer_.transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
 					flow_.integrated_xgyro = flow_gyro_fcu.vector.x;
 					flow_.integrated_ygyro = flow_gyro_fcu.vector.y;
 					flow_.integrated_zgyro = flow_gyro_fcu.vector.z;
 				} catch (const tf2::TransformException& e) {
+					prev_.release();
 					return;
 				}
 			}
@@ -201,7 +202,7 @@ private:
 			}
 
 			// Publish estimated angular velocity
-			static geometry_msgs::TwistStamped velo;
+			geometry_msgs::TwistStamped velo;
 			velo.header.stamp = msg->header.stamp;
 			velo.header.frame_id = fcu_frame_id_;
 			velo.twist.angular.x = flow_.integrated_x / integration_time.toSec();
@@ -223,8 +224,8 @@ private:
 		flow.header.frame_id = frame_id;
 		flow.header.stamp = curr;
 		auto diff = ((curr_rot - prev_rot) * prev_rot.inverse()) * 2.0f;
-		flow.vector.x = diff.x();
-		flow.vector.y = diff.y();
+		flow.vector.x = diff.y();
+		flow.vector.y = -diff.x();
 		flow.vector.z = diff.z();
 
 		return flow;