WIP attempting to get working transform

applications/registration/src/correspondances.cpp

@@ -123,8 +123,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
 
 		std::this_thread::sleep_for(std::chrono::milliseconds(50));
 	}
-	averageDepth(buffer[0], d1, 0.00000005f);
-	averageDepth(buffer[1], d2, 0.00000005f);
+	averageDepth(buffer[0], d1, 0.0000004f);
+	averageDepth(buffer[1], d2, 0.0000004f);
 	Mat d1_v, d2_v;
 	d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
 	d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
@@ -163,7 +163,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
 				p1.y = p1e[1];
 				p1.z = p1e[2];
 				src_cloud_->push_back(p1);
-				src_index_.at<int>(y,x) = six++;
+				src_index_.at<int>(y,x) = six;
+				six++;
 			}
 
 			if (d2_value < 39.0f) {
@@ -174,7 +175,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
 				p2.y = p2e[1];
 				p2.z = p2e[3];
 				targ_cloud_->push_back(p2);
-				targ_index_.at<int>(y,x) = tix++;
+				targ_index_.at<int>(y,x) = tix;
+				tix++;
 			}
 		}
 	}
@@ -221,8 +223,19 @@ double Correspondances::estimateTransform(Eigen::Matrix4f &T, const vector<int>
 	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
 	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
 
-	svd.estimateRigidTransformation(*src_cloud_, src_feat, *targ_cloud_, targ_feat, T);
-	return validate.validateTransformation(src_cloud_, targ_cloud_, T);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	vector<int> idx;
+
+	for (int i=0; i<src_feat.size(); i++) {
+		idx.push_back(i);
+		src_cloud->push_back(src_cloud_->at(src_feat[i]));
+		targ_cloud->push_back(targ_cloud_->at(targ_feat[i]));
+	}
+
+	svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
+	return validate.validateTransformation(src_cloud, targ_cloud, T);
 }
 
 static std::default_random_engine generator;
@@ -237,6 +250,42 @@ void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>
 	}
 }
 
+void Correspondances::getFeatures3D(std::vector<Eigen::Vector4f> &f) {
+	for (int i=0; i<src_feat_.size(); i++) {
+		Eigen::Vector4f p;
+		const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+		p[0] = pp.x;
+		p[1] = pp.y;
+		p[2] = pp.z;
+		p[3] = 1.0f;
+		f.push_back(p);
+	}
+}
+
+void Correspondances::getTransformedFeatures(std::vector<Eigen::Vector4f> &f) {
+	for (int i=0; i<src_feat_.size(); i++) {
+		Eigen::Vector4f p;
+		const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+		p[0] = pp.x;
+		p[1] = pp.y;
+		p[2] = pp.z;
+		p[3] = 1.0f;
+		f.push_back(transform_ * p);
+	}
+}
+
+void Correspondances::getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f) {
+	for (int i=0; i<src_feat_.size(); i++) {
+		Eigen::Vector4f p;
+		const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+		p[0] = pp.x;
+		p[1] = pp.y;
+		p[2] = pp.z;
+		p[3] = 1.0f;
+		f.push_back(src_->point(transform_ * p));
+	}
+}
+
 double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
 	double score = 10000.0f;
 	vector<tuple<int,int,int,int>> best;

applications/registration/src/correspondances.hpp

@@ -52,6 +52,9 @@ class Correspondances {
 	void removeLast();
 
 	const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
+	void getFeatures3D(std::vector<Eigen::Vector4f> &f);
+	void getTransformedFeatures(std::vector<Eigen::Vector4f> &f);
+	void getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f);
 
 	void setPoints(const std::vector<std::tuple<int,int,int,int>> &p) { log_ = p; }
 

applications/registration/src/manual.cpp

@@ -243,6 +243,14 @@ void ftl::registration::manual(ftl::Configurable *root) {
 				drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
 			}
 
+			vector<Eigen::Vector2i> tpoints;
+			current->getTransformedFeatures2D(tpoints);
+			for (int i=0; i<tpoints.size(); i++) {
+				Eigen::Vector2i p = tpoints[i];
+				drawMarker(trgb, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+				drawMarker(tdepth, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+			}
+
 			// Add most recent click position
 			drawMarker(srgb, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
 			drawMarker(trgb, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
@@ -318,12 +326,12 @@ void ftl::registration::manual(ftl::Configurable *root) {
 					lastScore = 1000.0;
 					do {
 						Eigen::Matrix4f tr;
-						float s = current->findBestSubset(tr, 6, 10);
+						float s = current->findBestSubset(tr, 20, 20);
 						LOG(INFO) << "SCORE = " << s;
 						if (s < lastScore) {
 							lastScore = s;
 							current->setTransform(tr);
-							current->source()->setPose(current->transform());
+							//current->source()->setPose(current->transform());
 						}
 					} while (ftl::running && insearch && lastScore > 0.000001);
 					insearch = false;

components/rgbd-sources/include/ftl/rgbd/source.hpp

@@ -139,6 +139,8 @@ class Source : public ftl::Configurable {
 	 */
 	Eigen::Vector4f point(uint x, uint y, float d);
 
+	Eigen::Vector2i point(const Eigen::Vector4f &p);
+
 	/**
 	 * Force the internal implementation to be reconstructed.
 	 */

components/rgbd-sources/src/source.cpp

@@ -164,6 +164,15 @@ Eigen::Vector4f Source::point(uint ux, uint uy, float d) {
 	return Eigen::Vector4f(x*d,y*d,d,1.0);
 }
 
+Eigen::Vector2i Source::point(const Eigen::Vector4f &p) {
+	const auto &params = parameters();
+	float x = p[0] / p[2];
+	float y = p[1] / p[2];
+	x *= params.fx;
+	y *= params.fy;
+	return Eigen::Vector2i((int)(x - params.cx), (int)(y - params.cy));
+}
+
 void Source::setPose(const Eigen::Matrix4f &pose) {
 	pose_ = pose;
 	if (impl_) impl_->setPose(pose);