Intermediate transfer commit - heavy WIP modules'

Author: Sai Vemprala

include/coloc/CPUMatcher.hpp

@@ -56,7 +56,7 @@ namespace coloc
 		bool matchMapFeatures(std::unique_ptr<features::AKAZE_Binary_Regions> &scene1, std::unique_ptr<features::AKAZE_Binary_Regions> &scene2, std::vector<IndMatch> &commonFeatures)
 		{
 			matching::DistanceRatioMatch(
-				0.9, BRUTE_FORCE_HAMMING,
+				0.8, BRUTE_FORCE_HAMMING,
 				*scene1.get(),
 				*scene2.get(),
 				commonFeatures);

include/coloc/GPUMatcher.hpp

@@ -159,7 +159,7 @@ namespace coloc
 			commonFeatures = computeMatches(const_cast<unsigned int*>(static_cast<const unsigned int*>(map1->DescriptorRawData())),
 				const_cast<unsigned int*>(static_cast<const unsigned int*>(map2->DescriptorRawData())),
 				map1->RegionCount(),
-				map2->RegionCount(), 70);
+				map2->RegionCount(), 60);
 		}
 
 		void computeMatchesPair(const Pair& pairIdx, FeatureMap& regions, IndMatches &putativeMatches)
@@ -186,10 +186,12 @@ namespace coloc
 			const size_t sizeDTraining = (numKPTraining + 8) * 64; // D2 for descriptor2
 
 			gpuErrchk(cudaMemsetAsync(d_descT, 0, sizeDTraining, m_stream1));
+			cudaStreamSynchronize(m_stream1);
 			gpuErrchk(cudaMemcpyAsync(d_descT, h_descriptorsTraining, sizeDTraining, cudaMemcpyHostToDevice, m_stream1));
 			cudaStreamSynchronize(m_stream1);			
 
 			gpuErrchk(cudaMemsetAsync(d_descQ, 0, sizeDQuery, m_stream2));
+			cudaStreamSynchronize(m_stream1);
 			gpuErrchk(cudaMemcpyAsync(d_descQ, h_descriptorsQuery, sizeDQuery, cudaMemcpyHostToDevice, m_stream2));
 			cudaStreamSynchronize(m_stream2);
 

include/coloc/InterfaceDisk.hpp

@@ -13,6 +13,7 @@ namespace coloc
 			std::string number = std::string(4 - std::to_string(imageNumber).length(), '0') + std::to_string(imageNumber);  //std::to_string(imageNumber); //std::string(4 - std::to_string(imageNumber).length(), '0') + std::to_string(imageNumber);
 			data->filenames[id] = params->imageFolder + "img__Quad" + std::to_string(id) + "_" + number + ".png";  //"image (" + number + ").png";
 			detector.detectFeaturesFile(id, data->regions, data->filenames[id]);
+			data->scene.views[id].reset(new View(data->filenames[id], id, 0, id, params->imageSize.first, params->imageSize.second));
 		}
 
 		void processImages(std::vector <int>& droneIds) override

include/coloc/KalmanFilter.hpp

@@ -60,13 +60,13 @@ namespace coloc
 
 			if (measurementsAvailable) {
 				bool reject = chiSquareGating(droneId, droneMeasurements[droneId], predicted);
-				//if (reject && !init) {
-				//	estimated = predicted;
-				//}
-				//else
-				//{
+				if (reject && !init) {
+					estimated = predicted;
+				}
+				else
+				{
 					estimated = droneFilters[droneId].correct(droneMeasurements[droneId]);
-				//}
+				}
 			}
 			else {
 				estimated = predicted;
@@ -106,7 +106,7 @@ namespace coloc
 		{
 			KF.init(nStates, nMeasurements, nInputs, CV_64F);                 // init Kalman Filter
 			cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5));       // set process noise
-			cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-3));   // set measurement noise
+			cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-4));   // set measurement noise
 			cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1));             // error covariance
 
 

include/coloc/Localizer.hpp

@@ -143,14 +143,14 @@ namespace coloc
 		cv::eigen2cv(pose.rotation(), rvec);
 		cv::Mat tvec = cv::Mat(1, 3, CV_32FC1, cv::Scalar::all(0));
 
-		tvec.at<float>(0, 0) = pose.center()[0];
-		tvec.at<float>(0, 1) = pose.center()[1];
-		tvec.at<float>(0, 2) = pose.center()[2];
+		tvec.at<float>(0, 0) = pose.translation()[0];
+		tvec.at<float>(0, 1) = pose.translation()[1];
+		tvec.at<float>(0, 2) = pose.translation()[2];
 
 		cv::Mat Kcv = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));
 		Kcv.at<float>(0, 0) = (*K)[idx](0, 0);
-		Kcv.at<float>(0, 2) = (*K)[idx](0, 0);
-		Kcv.at<float>(1, 1) = (*K)[idx](0, 2);
+		Kcv.at<float>(0, 2) = (*K)[idx](0, 2);
+		Kcv.at<float>(1, 1) = (*K)[idx](0, 0);
 		Kcv.at<float>(1, 2) = (*K)[idx](1, 2);
 		Kcv.at<float>(2, 2) = 1.0f;
 
@@ -167,16 +167,18 @@ namespace coloc
 			error += cv::norm(imagePoints[j] - reprojected[j]);
 		}
 
+		rmse = error / reprojected.size();
+
 		cv::Mat Sigma = cv::Mat(J.t() * J, cv::Rect(0, 0, 6, 6)).inv();
 		cv::Mat std_dev;
 		sqrt(Sigma.diag(), std_dev);
 		//std::cout << "Translation standard deviation:" << std::endl << std_dev.at<double>(3, 0) << std::endl << std_dev.at<double>(4, 0) << std::endl << std_dev.at<double>(5,0) << std::endl;
 
 		//std::cout << "Total standard deviation:" << std::endl << std_dev << std::endl;
 
-		poseCovariance[0][21] = std_dev.at<double>(3, 0);
-		poseCovariance[0][28] = std_dev.at<double>(4, 0);
-		poseCovariance[0][35] = std_dev.at<double>(5, 0);
+		//poseCovariance[0][21] = std_dev.at<double>(3, 0);
+		//poseCovariance[0][28] = std_dev.at<double>(4, 0);
+		//poseCovariance[0][35] = std_dev.at<double>(5, 0);
 
 		return refineStatus;
 	}

include/coloc/Reconstructor.hpp

@@ -44,6 +44,7 @@ namespace coloc
         {	}
 
         void reconstructScene(bool inter, colocData& data, std::vector <Pose3>, float, bool);
+		void Reconstructor::interReconstruct(int sourceId, int destId, colocData& data);
 
     private:
         // Internal methods
@@ -76,6 +77,28 @@ namespace coloc
         Scene* scene;
     };
 
+	void Reconstructor::interReconstruct(int sourceId, int destId, colocData& data)
+	{
+		this->regionsRCT = &data.regions;
+		this->matchesRCT = &data.geometricMatches;
+		this->relativePosesRCT = &data.relativePoses;
+		this->scene = &data.tempScene;
+
+
+		float scale = 1.0;
+		Pair seedPair = std::make_pair(sourceId, destId);
+
+		std::cout << "Using pair " << seedPair.first << " & " << seedPair.second << "as seed" << std::endl;
+
+		data.keyframeIdx = seedPair.first;
+		initializeTracks(seedPair);
+		initializeScene(imageSize->first, imageSize->second, seedPair);
+
+		std::cout << "Triangulating feature matches... ";
+		triangulatePoints(seedPair, Pose3(Mat3::Identity(), Vec3::Zero()), scale);
+		std::cout << "Done." << std::endl;
+	}
+
     void Reconstructor::reconstructScene(bool inter, colocData& data, std::vector <Pose3> poses, float scale = 1.0, bool Adjust = false)
     {
         this->regionsRCT = &data.regions;
@@ -183,8 +206,8 @@ namespace coloc
             const uint32_t i = iter->second;
             const uint32_t j = (++iter)->second;
 
-            const Vec2 x1_ = regionsRCT->at(pair.first)->GetRegionsPositions()[i].coords().cast<double>();
-            const Vec2 x2_ = regionsRCT->at(pair.second)->GetRegionsPositions()[j].coords().cast<double>();
+            const Vec2 x1_ = camCurrent->get_ud_pixel(regionsRCT->at(pair.first)->GetRegionsPositions()[i].coords().cast<double>());
+            const Vec2 x2_ = cam_J->get_ud_pixel(regionsRCT->at(pair.second)->GetRegionsPositions()[j].coords().cast<double>());
 
             Vec3 X;
 
@@ -203,7 +226,7 @@ namespace coloc
 			if (Pose_I.depth(X) < 0 && Pose_J.depth(X) < 0)
 				continue;
 
-			if (std::abs(X[2]) > 1000.0)
+			if (std::abs(X[2]) > 100.0)
 				continue;
 
             Observations obs;

include/coloc/RobustMatcher.hpp

@@ -160,7 +160,7 @@ namespace coloc
 
 			using KernelType = robust::ACKernelAdaptorEssential<
 				openMVG::essential::kernel::FivePointSolver,
-				openMVG::fundamental::kernel::EpipolarDistanceError,
+				openMVG::fundamental::kernel::SymmetricEpipolarDistanceError,
 				Mat3>;
 			KernelType kernel(x1, norm2Dpt_1, params.imageSize.first, params.imageSize.second,
 				x2, norm2Dpt_2, params.imageSize.first, params.imageSize.second,
@@ -170,7 +170,7 @@ namespace coloc
 			const auto ACRansacOut = ACRANSAC(kernel, relativePose_info.vec_inliers, iterationCount, &relativePose_info.essential_matrix,
 				relativePose_info.initial_residual_tolerance, false);
 
-			relativePose_info.found_residual_precision = 5.0;
+			relativePose_info.found_residual_precision = ACRansacOut.first;
 
 			if (relativePose_info.vec_inliers.size() < 2.5 * KernelType::Solver::MINIMUM_SAMPLES)
 				return EXIT_FAILURE;
@@ -243,9 +243,8 @@ namespace coloc
 			std::vector <IndMatch> putativeMatches, filteredMatches;
 
 			putativeMatches = commonFeatures;
-
 			commonFeatures.clear();
-
+			/*
 			const PointFeatures featI = scene1->GetRegionsPositions();
 			const PointFeatures featJ = scene2->GetRegionsPositions();
 
@@ -291,7 +290,8 @@ namespace coloc
 			//	*scene1.get(),
 			//	*scene2.get(),
 			//	putativeMatches);
-			/*
+			*/
+			
 			const PointFeatures featI = scene1->GetRegionsPositions();
 			const PointFeatures featJ = scene2->GetRegionsPositions();
 
@@ -344,7 +344,7 @@ namespace coloc
 					std::cout << "Right coordinates: " << xR.col(k) << std::endl;
 					
 				}
-				
+				*/
 				inliers.push_back(putativeMatches[k]);
 				myfile << res(0, 0) << "," << xL(0, k) << "," << xL(1, k) << "," << xR(0, k) << "," << xR(1, k) << std::endl;
 				epipolarLineDeviations.push_back(std::abs(res(0, 0)));
@@ -358,16 +358,18 @@ namespace coloc
 
 			std::sort(std::begin(matchIndices), std::end(matchIndices), [&](int i1, int i2) { return epipolarLineDeviations[i1] < epipolarLineDeviations[i2]; });
 			std::sort(epipolarLineDeviations.begin(), epipolarLineDeviations.end());
-			//for (int ic = 0; ic < inliers.size(); ++ic)
-			//	commonFeatures.push_back(inliers[ic]);
-
-			for (int ic = 0; ic < 2; ic++)
-				commonFeatures.push_back(putativeMatches[matchIndices[ic]]);
+			for (int ic = 0; ic < inliers.size(); ++ic)
+				commonFeatures.push_back(inliers[ic]);
+			/*
+			if (matchIndices.size() != 0) {
+				for (int ic = 0; ic < 2; ic++)
+					commonFeatures.push_back(putativeMatches[matchIndices[ic]]);
+			}
 			*/
 			return EXIT_SUCCESS;
 		}
 
-		std::unique_ptr <RelativePose_Info> computeRelativePose(Pair current_pair, FeatureMap& regions, PairWiseMatches& putativeMatches)
+		bool computeRelativePose(RelativePose_Info& relativePose, Pair current_pair, FeatureMap& regions, PairWiseMatches& putativeMatches)
 		{
 			const uint32_t I = std::min(current_pair.first, current_pair.second);
 			const uint32_t J = std::max(current_pair.first, current_pair.second);
@@ -378,12 +380,7 @@ namespace coloc
 			std::vector <IndMatch> pairMatches = putativeMatches[current_pair];
 			Mat xL(2, pairMatches.size());
 			Mat xR(2, pairMatches.size());
-			for (size_t k = 0; k < pairMatches.size(); ++k) {
-				xL.col(k) = featI[pairMatches[k].i_].coords().cast<double>();
-				xR.col(k) = featJ[pairMatches[k].j_].coords().cast<double>();
-			}
-
-			RelativePose_Info relativePose;
+					
 
 			const openMVG::cameras::Pinhole_Intrinsic_Radial_K3
 				camL(params->imageSize.first, params->imageSize.second, (params->K[current_pair.first])(0, 0), (params->K[current_pair.first])(0, 2), (params->K[current_pair.first])(1, 2), 
@@ -393,6 +390,13 @@ namespace coloc
 
 			bool status, findPose = true;
 
+			for (size_t k = 0; k < pairMatches.size(); ++k) {
+				xL.col(k) = featI[pairMatches[k].i_].coords().cast<double>();
+				xL.col(k) = camL.get_ud_pixel(xL.col(k));
+				xR.col(k) = featJ[pairMatches[k].j_].coords().cast<double>();
+				xR.col(k) = camR.get_ud_pixel(xR.col(k));
+			}
+
 			if (params->model == 'H')
 				status = filterHomography(&camL, &camR, xL, xR, relativePose, *params, findPose);
 			else if (params->model == 'E')
@@ -416,19 +420,50 @@ namespace coloc
 					<< std::endl;
 			}
 
-			return std::make_unique<RelativePose_Info>(relativePose);
+			return status;
+		}
+
+		bool filterMatchesPair(Pair currentPair, FeatureMap& regions, PairWiseMatches& putativeMatches, PairWiseMatches& geometricMatches, InterPoseMap& relativePoses)
+		{
+			std::vector <IndMatch> pairMatches = putativeMatches[currentPair];
+			RelativePose_Info relativePose;
+			
+			bool status = computeRelativePose(relativePose, currentPair, regions, putativeMatches);
+
+			std::vector <IndMatch> vec_geometricMatches;
+			for (int ic = 0; ic < relativePose.vec_inliers.size(); ++ic)
+				vec_geometricMatches.push_back(pairMatches[relativePose.vec_inliers[ic]]);
+
+			if (!vec_geometricMatches.empty())
+			{
+				PairWiseMatches::iterator it = geometricMatches.find(currentPair);
+				if (geometricMatches.end() != it)
+					geometricMatches.at(currentPair) = vec_geometricMatches;
+				else
+					geometricMatches.insert({ { currentPair.first, currentPair.second }, std::move(vec_geometricMatches) });
+			}
+
+			InterPoseMap::iterator it = relativePoses.find(currentPair);
+			if (relativePoses.end() != it)
+				relativePoses.at(currentPair) = relativePose;
+			else
+				relativePoses.insert({ { currentPair.first, currentPair.second }, std::move(relativePose) });
+			
+			return status;
 		}
 
 		void filterMatches(FeatureMap& regions, PairWiseMatches& putativeMatches, PairWiseMatches& geometricMatches, InterPoseMap& relativePoses)
 		{
 			for (const auto matchedPair : putativeMatches) {
 				Pair currentPair = matchedPair.first;
 				std::vector <IndMatch> pairMatches = putativeMatches[currentPair];
-				std::unique_ptr<RelativePose_Info> relativePose = computeRelativePose(matchedPair.first, regions, putativeMatches);
+				RelativePose_Info relativePose;
+				
+				bool status = computeRelativePose(relativePose, matchedPair.first, regions, putativeMatches);
 
 				std::vector <IndMatch> vec_geometricMatches;
-				for (int ic = 0; ic < relativePose->vec_inliers.size(); ++ic)
-					vec_geometricMatches.push_back(pairMatches[relativePose->vec_inliers[ic]]);
+				for (int ic = 0; ic < relativePose.vec_inliers.size(); ++ic)
+					vec_geometricMatches.push_back(pairMatches[relativePose.vec_inliers[ic]]);
 
 				if (!vec_geometricMatches.empty())
 				{
@@ -441,9 +476,9 @@ namespace coloc
 
 				InterPoseMap::iterator it = relativePoses.find(currentPair);
 				if (relativePoses.end() != it)
-					relativePoses.at(currentPair) = *relativePose;
+					relativePoses.at(currentPair) = relativePose;
 				else
-					relativePoses.insert({ { currentPair.first, currentPair.second }, std::move(*relativePose) });
+					relativePoses.insert({ { currentPair.first, currentPair.second }, std::move(relativePose) });
 			}
 		}
 	};