Ongoing #133 improvements

applications/reconstruct/src/ilw/ilw.cpp

@@ -81,6 +81,9 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
 
 	// For each camera combination
     for (size_t i=0; i<fs.frames.size(); ++i) {
+		Eigen::Vector4d d1(0.0, 0.0, 1.0, 0.0);
+		d1 = fs.sources[i]->getPose() * d1;
+
         for (size_t j=0; j<fs.frames.size(); ++j) {
             if (i == j) continue;
 
@@ -91,6 +94,12 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
             //auto s1 = fs.frames[i];
             auto s2 = fs.sources[j];
 
+			// Are cameras facing similar enough direction?
+			Eigen::Vector4d d2(0.0, 0.0, 1.0, 0.0);
+			d2 = fs.sources[j]->getPose() * d2;
+			// No, so skip this combination
+			if (d1.dot(d2) <= 0.0) continue;
+
             auto pose = MatrixConversion::toCUDA(s2->getPose().cast<float>().inverse());
 
             try {