Resolves #115 in groupview app

applications/groupview/src/main.cpp

@@ -4,6 +4,38 @@
 #include <ftl/rgbd/source.hpp>
 #include <ftl/rgbd/group.hpp>
 
+#ifdef HAVE_LIBARCHIVE
+#include <ftl/rgbd/snapshot.hpp>
+#endif
+
+#include <fstream>
+
+using Eigen::Matrix4d;
+using std::map;
+using std::string;
+
+static void from_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
+	for (auto it = json.begin(); it != json.end(); ++it) {
+		Eigen::Matrix4d m;
+		auto data = m.data();
+		for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
+		transformations[it.key()] = m;
+	}
+}
+
+static bool loadTransformations(const string &path, map<string, Matrix4d> &data) {
+	std::ifstream file(path);
+	if (!file.is_open()) {
+		LOG(ERROR) << "Error loading transformations from file " << path;
+		return false;
+	}
+	
+	nlohmann::json json_registration;
+	file >> json_registration;
+	from_json(json_registration, data);
+	return true;
+}
+
 int main(int argc, char **argv) {
 	auto root = ftl::configure(argc, argv, "viewer_default");
 	ftl::net::Universe *net = ftl::create<ftl::net::Universe>(root, "net");
@@ -13,20 +45,63 @@ int main(int argc, char **argv) {
 
 	auto sources = ftl::createArray<ftl::rgbd::Source>(root, "sources", net);
 
+	std::map<std::string, Eigen::Matrix4d> transformations;
+	if (loadTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
+		LOG(INFO) << "Loaded camera trasformations from file";
+	}
+	else {
+		LOG(ERROR) << "Error loading camera transformations from file";
+	}
+
 	ftl::rgbd::Group group;
 	for (auto s : sources) {
-		s->setChannel(ftl::rgbd::kChanRight);
+		string uri = s->getURI();
+		auto T = transformations.find(uri);
+		if (T == transformations.end()) {
+			LOG(ERROR) << "Camera pose for " + uri + " not found in transformations";
+		} else {
+			s->setPose(T->second);
+		}
+		s->setChannel(ftl::rgbd::kChanDepth);
 		group.addSource(s);
 	}
 
-	group.sync([](const ftl::rgbd::FrameSet &fs) {
+	bool grab = false;
+
+	group.sync([&grab](const ftl::rgbd::FrameSet &fs) {
 		LOG(INFO) << "Complete set: " << fs.timestamp;
+		if (grab) {
+			grab = false;
+
+#ifdef HAVE_LIBARCHIVE
+			char timestamp[18];
+			std::time_t t=std::time(NULL);
+			std::strftime(timestamp, sizeof(timestamp), "%F-%H%M%S", std::localtime(&t));
+			auto writer = ftl::rgbd::SnapshotWriter(std::string(timestamp) + ".tar.gz");
+
+			for (size_t i=0; i<fs.sources.size(); ++i) {
+				writer.addCameraParams(std::string("camera")+std::to_string(i), fs.sources[i]->getPose(), fs.sources[i]->parameters());
+				LOG(INFO) << "SAVE: " << fs.channel1[i].cols << ", " << fs.channel2[i].type();
+				writer.addCameraRGBD(std::string("camera")+std::to_string(i), fs.channel1[i], fs.channel2[i]);
+			}
+#endif  // HAVE_LIBARCHIVE
+		}
 		return true;
 	});
 
+	int current = 0;
+
 	while (ftl::running) {
-		std::this_thread::sleep_for(std::chrono::milliseconds(20));
+		//std::this_thread::sleep_for(std::chrono::milliseconds(20));
 		for (auto s : sources) s->grab(30);
+		cv::Mat rgb,depth;
+		sources[current%sources.size()]->getFrames(rgb, depth);
+		if (!rgb.empty()) cv::imshow("View", rgb);
+		auto key = cv::waitKey(20);
+
+		if (key == 27) break;
+		if (key == 'n') current++;
+		if (key == 'g') grab = true;
 	}
 
 	return 0;

applications/reconstruct/src/main.cpp

@@ -79,11 +79,12 @@ static void run(ftl::Configurable *root) {
 			auto T = transformations.find(uri);
 			if (T == transformations.end()) {
 				LOG(ERROR) << "Camera pose for " + uri + " not found in transformations";
-				LOG(WARNING) << "Using only first configured source";
+				//LOG(WARNING) << "Using only first configured source";
 				// TODO: use target source if configured and found
-				sources = { sources[0] };
-				sources[0]->setPose(Eigen::Matrix4d::Identity());
-				break;
+				//sources = { sources[0] };
+				//sources[0]->setPose(Eigen::Matrix4d::Identity());
+				//break;
+				continue;
 			}
 			input->setPose(T->second);
 		}

components/rgbd-sources/src/group.cpp

@@ -105,6 +105,7 @@ void Group::_addFrameset(int64_t timestamp) {
 		framesets_[head_].channel1.resize(sources_.size());
 		framesets_[head_].channel2.resize(sources_.size());
 
+		framesets_[head_].sources.clear();
 		for (auto s : sources_) framesets_[head_].sources.push_back(s);
 	}
 }

components/rgbd-sources/src/net.cpp

@@ -130,15 +130,20 @@ void NetSource::_recvChunk(int64_t frame, int chunk, bool delta, const vector<un
 	// Make certain last frame has finished decoding before swap
 	while (frame > current_frame_ && chunk_count_ < 16 && chunk_count_ > 0) {
 		std::this_thread::yield();
+		//std::function<void(int)> j = ftl::pool.pop();
+		//if ((bool)j) j(-1);
+		//else std::this_thread::yield();
 	}
 
-	// Lock host to prevent grab
-	UNIQUE_LOCK(host_->mutex(),lk);
-
+	//{
 		// A new frame has been started... finish the last one
 		if (frame > current_frame_) {
 			// Lock host to prevent grab
-		//UNIQUE_LOCK(host_->mutex(),lk);
+			UNIQUE_LOCK(host_->mutex(),lk);
+			if (frame > current_frame_) {
+				{
+					// Lock to allow buffer swap
+					UNIQUE_LOCK(mutex_,lk2);
 
 					chunk_count_ = 0;
 
@@ -153,6 +158,7 @@ void NetSource::_recvChunk(int64_t frame, int chunk, bool delta, const vector<un
 
 					timestamp_ = current_frame_*40;  // FIXME: Don't hardcode 40ms
 					current_frame_ = frame;
+				}
 
 				if (host_->callback()) {
 					//ftl::pool.push([this](id) {
@@ -160,14 +166,19 @@ void NetSource::_recvChunk(int64_t frame, int chunk, bool delta, const vector<un
 						host_->callback()(timestamp_, rgb_, depth_);
 					//});
 				}
+			}
 		} else if (frame < current_frame_) {
 			LOG(WARNING) << "Chunk dropped";
 			if (chunk == 0) N_--;
 			return;
 		}
+	//}
 
 	// TODO:(Nick) Decode directly into double buffer if no scaling
 
+	{
+		SHARED_LOCK(mutex_, lk);
+		
 		cv::Rect roi(cx,cy,chunk_width_,chunk_height_);
 		cv::Mat chunkRGB = d_rgb_(roi);
 		cv::Mat chunkDepth = d_depth_(roi);
@@ -195,9 +206,15 @@ void NetSource::_recvChunk(int64_t frame, int chunk, bool delta, const vector<un
 				// Silent ignore?
 			}
 		}
+	}
+
+	{
 		
 		++chunk_count_;
+	}
+
 	if (chunk == 0) {
+		UNIQUE_LOCK(host_->mutex(),lk);
 		N_--;
 	}
 }

components/rgbd-sources/src/net.hpp

@@ -39,7 +39,7 @@ class NetSource : public detail::Source {
 	bool active_;
 	std::string uri_;
 	ftl::net::callback_t h_;
-	MUTEX mutex_;
+	SHARED_MUTEX mutex_;
 	int chunks_dim_;
 	int chunk_width_;
 	int chunk_height_;
@@ -51,7 +51,7 @@ class NetSource : public detail::Source {
 	int default_quality_;
 	ftl::rgbd::channel_t prev_chan_;
 	int64_t current_frame_;
-	int chunk_count_;
+	std::atomic<int> chunk_count_;
 
 	// Double buffering
 	cv::Mat d_depth_;

components/rgbd-sources/src/snapshot.cpp

@@ -275,11 +275,12 @@ bool SnapshotReader::readArchive() {
 		else if (path.rfind("-D.") != string::npos) {
 			if (!readEntry(data)) continue;
 			snapshot.depth = cv::imdecode(data, cv::IMREAD_ANYDEPTH);
+			snapshot.depth.convertTo(snapshot.depth, CV_32FC1, 1.0f / 1000.0f);
 			snapshot.status &= ~(1 << 1);
 		}
 		else if (path.rfind("-POSE.pfm") != string::npos) {
 			if (!readEntry(data)) continue;
-			Mat m_ = cv::imdecode(Mat(data), 0);
+			Mat m_ = cv::imdecode(Mat(data), cv::IMREAD_ANYDEPTH);
 			if ((m_.rows != 4) || (m_.cols != 4)) continue;
 			cv::Matx44d pose_(m_);
 			cv::cv2eigen(pose_, snapshot.pose);

components/rgbd-sources/src/snapshot_source.cpp

@@ -17,6 +17,10 @@ SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader,
     Eigen::Matrix4d pose;
     reader.getCameraRGBD(id, rgb_, depth_, pose, params_);
 
+	if (rgb_.empty()) LOG(ERROR) << "Did not load snapshot rgb - " << id;
+	if (depth_.empty()) LOG(ERROR) << "Did not load snapshot depth - " << id;
+	if (params_.width != rgb_.cols) LOG(ERROR) << "Camera parameters corrupt for " << id;
+
 	ftl::rgbd::colourCorrection(rgb_, host->value("gamma", 1.0f), host->value("temperature", 6500));
 
 	host->on("gamma", [this,host](const ftl::config::Event&) {
@@ -42,5 +46,7 @@ SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader,
 		params_.cy = host_->value("centre_y", params_.cy);
 	});
 
-    setPose(pose);
+	LOG(INFO) << "POSE = " << pose;
+
+    host->setPose(pose);
 }

components/rgbd-sources/src/source.cpp

@@ -120,7 +120,7 @@ ftl::rgbd::detail::Source *Source::_createFileImpl(const ftl::URI &uri) {
 		} else if (ext == "tar" || ext == "gz") {
 #ifdef HAVE_LIBARCHIVE
 			ftl::rgbd::SnapshotReader reader(path);
-			return new ftl::rgbd::detail::SnapshotSource(this, reader, std::to_string(value("index", 0)));  // TODO: Use URI fragment
+			return new ftl::rgbd::detail::SnapshotSource(this, reader, value("index", std::string("0")));  // TODO: Use URI fragment
 #else
 			LOG(ERROR) << "Cannot read snapshots, libarchive not installed";
 			return nullptr;
@@ -231,6 +231,15 @@ bool Source::grab(int N, int B) {
 		return true;
 	} else if (impl_ && impl_->grab(N,B)) {
 		timestamp_ = impl_->timestamp_;
+		/*cv::Mat tmp;
+		rgb_.create(impl_->rgb_.size(), impl_->rgb_.type());
+		depth_.create(impl_->depth_.size(), impl_->depth_.type());
+		tmp = rgb_;
+		rgb_ = impl_->rgb_;
+		impl_->rgb_ = tmp;
+		tmp = depth_;
+		depth_ = impl_->depth_;
+		impl_->depth_ = tmp;*/
 		impl_->rgb_.copyTo(rgb_);
 		impl_->depth_.copyTo(depth_);
 		return true;

config/config_nick.jsonc

@@ -128,6 +128,7 @@
 				"SDFUseGradients": false,
 				"showBlockBorders": false
 			},
+			"baseline": 0.5,
 			"uri": "device:virtual"
 		}
 	},
@@ -146,10 +147,10 @@
 			"colourSmoothing": 200.0,
 			"colourInfluence": 2.0,
 			"spatialSmoothing": 0.04,
-			"confidenceThreshold": 0.00001,
-			"mls": false,
+			"confidenceThreshold": 0.0,
+			"mls": true,
 			"voxels": false,
-			"clipping": true,
+			"clipping": false,
 			"bbox_x_max": 1.5,
 			"bbox_x_min": -1.5,
 			"bbox_y_max": 3.0,
@@ -192,12 +193,19 @@
 
 	"viewer_default": {
 		"net": {
-			"peers": ["tcp://ftl-node-4:9001", "tcp://ftl-node-3:9001", "tcp://ftl-node-1:9001"]
+			"peers": ["tcp://ftl-node-4:9001",
+				"tcp://ftl-node-5:9001",
+				"tcp://ftl-node-1:9001",
+				"tcp://ftl-node-6:9001",
+				"tcp://ftl-node-3:9001"],
+			"listen": "tcp://*:9001"
 		},
 		"sources": [
+			{"uri":"ftl://utu.fi/node1#vision_default/source"},
+			{"uri":"ftl://utu.fi/node6#vision_default/source"},
+			//{"uri":"ftl://utu.fi/node3#vision_default/source"},
 			{"uri":"ftl://utu.fi/node4#vision_default/source"},
-			{"uri":"ftl://utu.fi/node3#vision_default/source"},
-			{"uri":"ftl://utu.fi/node1#vision_default/source"}
+			{"uri":"ftl://utu.fi/node5#vision_default/source"}
 		]
 	},
 
@@ -310,6 +318,195 @@
 		}
 	},
 
+	"reconstruction_snap2": {
+		"net": {
+			"peers": [],
+			"listen": "tcp://*:9001"
+		},
+		"sources": [
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest2.tar.gz#0", "index": "camera0"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest2.tar.gz#1", "index": "camera1"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest2.tar.gz#2", "index": "camera2"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest2.tar.gz#3", "index": "camera3"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"merge": {
+			"$id": "ftl://blah/blah",
+			"targetsource" : "ftl://utu.fi/node3#vision_default/source",
+			"register": false,
+			"chain": false,
+			"maxerror": 100,
+			"iterations" : 10,
+			"delay" : 500,
+			"patternsize" : [9, 6]
+		},
+		"stream": {}
+	},
+
+	"reconstruction_snap3": {
+		"net": {
+			"peers": [],
+			"listen": "tcp://*:9001"
+		},
+		"sources": [
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest3.tar.gz#0", "index": "camera0"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest3.tar.gz#1", "index": "camera1"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest3.tar.gz#2", "index": "camera2"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest3.tar.gz#3", "index": "camera3"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"merge": {
+			"$id": "ftl://blah/blah",
+			"targetsource" : "ftl://utu.fi/node3#vision_default/source",
+			"register": false,
+			"chain": false,
+			"maxerror": 100,
+			"iterations" : 10,
+			"delay" : 500,
+			"patternsize" : [9, 6]
+		},
+		"stream": {}
+	},
+
+	"reconstruction_snap4": {
+		"net": {
+			"peers": [],
+			"listen": "tcp://*:9001"
+		},
+		"sources": [
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest4.tar.gz#0", "index": "camera0"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest4.tar.gz#1", "index": "camera1"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest4.tar.gz#2", "index": "camera2"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest4.tar.gz#3", "index": "camera3"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"merge": {
+			"$id": "ftl://blah/blah",
+			"targetsource" : "ftl://utu.fi/node3#vision_default/source",
+			"register": false,
+			"chain": false,
+			"maxerror": 100,
+			"iterations" : 10,
+			"delay" : 500,
+			"patternsize" : [9, 6]
+		},
+		"stream": {}
+	},
+
+	"reconstruction_snap5": {
+		"net": {
+			"peers": [],
+			"listen": "tcp://*:9001"
+		},
+		"sources": [
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest5.tar.gz#0", "index": "camera0"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest5.tar.gz#1", "index": "camera1"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest5.tar.gz#2", "index": "camera2"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest5.tar.gz#3", "index": "camera3"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"merge": {
+			"$id": "ftl://blah/blah",
+			"targetsource" : "ftl://utu.fi/node3#vision_default/source",
+			"register": false,
+			"chain": false,
+			"maxerror": 100,
+			"iterations" : 10,
+			"delay" : 500,
+			"patternsize" : [9, 6]
+		},
+		"stream": {}
+	},
+
+	"reconstruction_snap6": {
+		"net": {
+			"peers": [],
+			"listen": "tcp://*:9001"
+		},
+		"sources": [
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest6.tar.gz#0", "index": "camera0"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest6.tar.gz#1", "index": "camera1"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest6.tar.gz#2", "index": "camera2"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest6.tar.gz#3", "index": "camera3"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"merge": {
+			"$id": "ftl://blah/blah",
+			"targetsource" : "ftl://utu.fi/node3#vision_default/source",
+			"register": false,
+			"chain": false,
+			"maxerror": 100,
+			"iterations" : 10,
+			"delay" : 500,
+			"patternsize" : [9, 6]
+		},
+		"stream": {}
+	},
+
+	"reconstruction_snap7": {
+		"net": {
+			"peers": [],
+			"listen": "tcp://*:9001"
+		},
+		"sources": [
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest7.tar.gz#0", "index": "camera0"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest7.tar.gz#1", "index": "camera1"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest7.tar.gz#2", "index": "camera2"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest7.tar.gz#3", "index": "camera3"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"merge": {
+			"$id": "ftl://blah/blah",
+			"targetsource" : "ftl://utu.fi/node3#vision_default/source",
+			"register": false,
+			"chain": false,
+			"maxerror": 100,
+			"iterations" : 10,
+			"delay" : 500,
+			"patternsize" : [9, 6]
+		},
+		"stream": {}
+	},
+
+	"reconstruction_snap8": {
+		"net": {
+			"peers": [],
+			"listen": "tcp://*:9001"
+		},
+		"sources": [
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest8.tar.gz#0", "index": "camera0"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest8.tar.gz#1", "index": "camera1"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest8.tar.gz#2", "index": "camera2"},
+			{"uri":"file:///home/nick/Pictures/FTL/snaptest8.tar.gz#3", "index": "camera3"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"merge": {
+			"$id": "ftl://blah/blah",
+			"targetsource" : "ftl://utu.fi/node3#vision_default/source",
+			"register": false,
+			"chain": false,
+			"maxerror": 100,
+			"iterations" : 10,
+			"delay" : 500,
+			"patternsize" : [9, 6]
+		},
+		"stream": {}
+	},
+
 	"reconstruction_lab": {
 		"net": {
 			"peers": ["tcp://ftl-node-4:9001",
@@ -428,16 +625,16 @@
 			"peers": ["tcp://ftl-node-4:9001",
 				"tcp://ftl-node-5:9001",
 				"tcp://ftl-node-1:9001",
-				"tcp://ftl-node-2:9001",
+				"tcp://ftl-node-6:9001",
 				"tcp://ftl-node-3:9001"],
 			"listen": "tcp://*:9001"
 		},
 		"sources": [
-			{"uri":"ftl://utu.fi/node1#vision_default/source", "gamma": 0.8, "temperature": 6500, "scaling": 1.0},
-			{"uri":"ftl://utu.fi/node2#vision_default/source", "gamma": 1.2, "temperature": 6500, "scaling": 1.0},
-			{"uri":"ftl://utu.fi/node3#vision_default/source", "gamma": 1.2, "temperature": 6500, "scaling": 1.0},
-			{"uri":"ftl://utu.fi/node4#vision_default/source", "gamma": 1.2, "temperature": 6500, "scaling": 1.0},
-			{"uri":"ftl://utu.fi/node5#vision_default/source", "gamma": 1.2, "temperature": 6500, "scaling": 1.0}
+			{"uri":"ftl://utu.fi/node1#vision_default/source"},
+			{"uri":"ftl://utu.fi/node6#vision_default/source"},
+			//{"uri":"ftl://utu.fi/node3#vision_default/source"},
+			{"uri":"ftl://utu.fi/node4#vision_default/source"},
+			{"uri":"ftl://utu.fi/node5#vision_default/source"}
 		],
 		"display": { "$ref": "#displays/left" },
 		"virtual": { "$ref": "#virtual_cams/default" },