Fixes to depth smooth and send pose

applications/registration/src/manual.cpp

@@ -148,6 +148,8 @@ void averageDepth(vector<Mat> &in, Mat &out) {
 
 		if (good_values > 0) {
 			out.at<float>(i) = sum / (float) good_values;
+		} else {
+			out.at<float>(i) = 41.0f;
 		}
 	}
 }
@@ -335,6 +337,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
 						if (s < lastScore) {
 							lastScore = s;
 							current->setTransform(tr);
+							current->source()->setPose(current->transform());
 						}
 					} while (ftl::running && insearch && lastScore > 0.000001);
 					insearch = false;
@@ -356,10 +359,14 @@ void ftl::registration::manual(ftl::Configurable *root) {
 			vector<vector<Mat>> buffer(2, vector<Mat>(buffer_size));
 
 			for (size_t i = 0; i < buffer_size; ++i) {
+				current->source()->grab();
+				current->target()->grab();
 				current->source()->getFrames(f1, d1);
 				current->target()->getFrames(f2, d2);
 				buffer[0][i] = d1;
 				buffer[1][i] = d2;
+
+				std::this_thread::sleep_for(std::chrono::milliseconds(20));
 			}
 			averageDepth(buffer[0], d1);
 			averageDepth(buffer[1], d2);
@@ -369,8 +376,8 @@ void ftl::registration::manual(ftl::Configurable *root) {
 			applyColorMap(d1_v, d1_v, cv::COLORMAP_JET);
 			applyColorMap(d2_v, d2_v, cv::COLORMAP_JET);
 
-			cv::imshow("smooth d1", d1);
-			cv::imshow("smooth d2", d2);
+			cv::imshow("smooth d1", d1_v);
+			cv::imshow("smooth d2", d2_v);
 
 			Ptr<xphoto::WhiteBalancer> wb;
 			wb = xphoto::createSimpleWB();