Skip cameras facing wrong way

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 {

applications/reconstruct/src/ilw/ilw.cu

@@ -57,8 +57,8 @@ __global__ void correspondence_energy_vector_kernel(
 
         // Determine degree of correspondence
         float cost = 1.0f - ftl::cuda::spatialWeighting(world1, world2, 0.04f);
-        cost += 1.0f - ftl::cuda::colourWeighting(colour1, colour2, 50.0f);
-        cost /= 2.0f;
+        cost *= 1.0f - ftl::cuda::colourWeighting(colour1, colour2, 50.0f);
+        //cost /= 2.0f;
 
         if (cost < bestcost) {
             bestpoint = world2;
@@ -77,8 +77,10 @@ __global__ void correspondence_energy_vector_kernel(
             (bestpoint.x - world1.x),
             (bestpoint.y - world1.y),
             (bestpoint.z - world1.z),
-            mincost);
-		eout(x,y) = max(eout(x,y), (1.0f - mincost) * 7.0f); //confidence * 5.0f;
+			mincost);
+			
+		eout(x,y) = max(eout(x,y), length(bestpoint-world1) * 10.0f);
+		//eout(x,y) = max(eout(x,y), (1.0f - mincost) * 7.0f); //confidence * 5.0f;
 
 		// FIXME: This needs to be summed across all frames
         //eout(x,y) = max(eout(x, y), confidence * 7.0f);