Show Base Camera Reprojected Points

ajingu · ajingu · commit f1f5a6e875a7 · 2019-05-23T12:29:01.000+09:00
diff --git a/Common/Correspondence/test2/Camera_Transform.xml b/Common/Correspondence/test2/Camera_Transform.xml
@@ -17,13 +17,13 @@
   <cols>1</cols>
   <dt>d</dt>
   <data>
-    -7.2324845629976139e-02 -3.9142336638089262e-02
-    6.1135124121830288e-03</data></R1>
+    -2.5000198851341271e-01 3.8329953249649891e-02
+    8.3816804829036501e-03</data></R1>
 <t1 type_id="opencv-matrix">
   <rows>3</rows>
   <cols>1</cols>
   <dt>d</dt>
   <data>
-    1.5376839119543745e-01 -9.1043479526995118e-03
-    3.4149801732138363e-03</data></t1>
+    1.3414331953572689e-01 -6.5430910963729716e-02
+    -1.3773779290331608e-02</data></t1>
 </opencv_storage>
diff --git a/Common/Correspondence/test2/point3d.txt b/Common/Correspondence/test2/point3d.txt
@@ -1,17 +1,33 @@
-16
--0.140011 -0.0207075 0.313244
--0.108098 -0.0207466 0.310889
--0.109063 0.00822113 0.297327
--0.140976 0.00826023 0.299682
--0.0738667 -0.0213791 0.30834
--0.0420502 -0.021456 0.304919
--0.0433975 0.00767033 0.291734
--0.075214 0.00774727 0.295156
--0.141312 0.0155673 0.295765
--0.109348 0.0158015 0.294273
--0.110199 0.0447066 0.28057
--0.142164 0.0444724 0.282062
--0.074936 0.0151192 0.292574
--0.043024 0.0152506 0.290206
--0.0441224 0.0443856 0.277017
--0.0760344 0.0442542 0.279385
+32
+-0.14473 -0.0228553 0.322448
+-0.112737 -0.02284 0.321793
+-0.112922 0.00805029 0.31344
+-0.144916 0.00803501 0.314096
+-0.0783766 -0.0241174 0.325577
+-0.0463961 -0.0244417 0.324508
+-0.0463221 0.00672937 0.317272
+-0.0783026 0.00705359 0.318341
+-0.14457 0.0159571 0.312138
+-0.112576 0.0160648 0.311562
+-0.112853 0.0465525 0.301845
+-0.144848 0.0464448 0.302421
+-0.0787003 0.0145954 0.313841
+-0.0467259 0.0141964 0.315058
+-0.046094 0.045559 0.308734
+-0.0780684 0.0459579 0.307517
+-0.14473 -0.0228553 0.322448
+-0.112737 -0.02284 0.321793
+-0.112922 0.00805029 0.31344
+-0.144916 0.00803501 0.314096
+-0.0783766 -0.0241174 0.325577
+-0.0463961 -0.0244417 0.324508
+-0.0463221 0.00672937 0.317272
+-0.0783026 0.00705359 0.318341
+-0.14457 0.0159571 0.312138
+-0.112576 0.0160648 0.311562
+-0.112853 0.0465525 0.301845
+-0.144848 0.0464448 0.302421
+-0.0787003 0.0145954 0.313841
+-0.0467259 0.0141964 0.315058
+-0.046094 0.045559 0.308734
+-0.0780684 0.0459579 0.307517
diff --git a/Test2_BundleAdjustment/bundle_adjustmenter.cpp b/Test2_BundleAdjustment/bundle_adjustmenter.cpp
@@ -71,7 +71,7 @@ class BALProblem
 
 	void getPoint3dCoordinates(vector<Point3d>& points)
 	{
-		for (int i = num_base_camera_observations(); i < num_observations(); i++)
+		for (int i = 0; i < num_observations(); i++)
 		{
 			double* base_marker_transform = mutable_base_marker_transform_from_base_camera(i);
 			double* marker_transform = mutable_marker_transform_from_base_marker(i);
diff --git a/Test2_BundleAdjustment/main.cpp b/Test2_BundleAdjustment/main.cpp
@@ -85,7 +85,7 @@ int main(int argc, char** argv)
 			bal_problem.mutable_camera_transform_from_base_camera(i),
 			bal_problem.mutable_base_marker_transform_from_base_camera(i),
 			bal_problem.mutable_marker_transform_from_base_marker(i));
-		
+		/*
 		for (int j = 0; j < num_base_camera_observation; j++)
 		{
 			CostFunction* base_camera_cost_function =
@@ -99,7 +99,7 @@ int main(int argc, char** argv)
 				NULL,
 				bal_problem.mutable_base_marker_transform_from_base_camera(j),
 				bal_problem.mutable_marker_transform_from_base_marker(j));
-		}
+		}*/
 	}
 	
 	Solver::Options options;
diff --git a/Test2_ReprojectionCheck/main.cpp b/Test2_ReprojectionCheck/main.cpp
@@ -11,10 +11,47 @@
 #include <opencv2/aruco.hpp>
 
 #define MARKER_SIDE 0.032
+#define CAMERAS 2
 
 using namespace std;
 using namespace cv;
 
+vector<Point3d> getCornersInCameraWorld(double side, Vec3d rvec, Vec3d tvec)
+{
+	double half_side = side / 2;
+
+	// compute rot_mat
+	Mat rot_mat;
+	Rodrigues(rvec, rot_mat);
+
+	// transpose of rot_mat for easy columns extraction
+	Mat rot_mat_t = rot_mat.t();
+
+	// the two E-O and F-O vectors
+	double * tmp = rot_mat_t.ptr<double>(0);
+	Point3d camWorldE(tmp[0] * half_side,
+		tmp[1] * half_side,
+		tmp[2] * half_side);
+
+	tmp = rot_mat_t.ptr<double>(1);
+	Point3d camWorldF(tmp[0] * half_side,
+		tmp[1] * half_side,
+		tmp[2] * half_side);
+
+	// convert tvec to point
+	Point3d tvec_3d(tvec[0], tvec[1], tvec[2]);
+
+	// return vector:
+	vector<Point3d> ret(4, tvec_3d);
+
+	ret[0] += -camWorldE + camWorldF; //top left
+	ret[1] += camWorldE + camWorldF; //top right
+	ret[2] += camWorldE - camWorldF; //bottom right
+	ret[3] += -camWorldE - camWorldF; //bottom left
+
+	return ret;
+}
+
 int main()
 {
 	//get intrinsics
@@ -51,13 +88,14 @@ int main()
 	//get marker points
 	Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_4X4_100);
 	//vector<Point3d> object_points;
-	vector<Point2f> image_points;
+	vector<Point2f> image_points[CAMERAS];
 
 	map<string, Mat> images;
 
 
-	for_each(begin(serial_numbers), end(serial_numbers), [&dictionary, /*&object_points,*/ &image_points, &serial_numbers, &camera_matrix_map, &dist_coeffs_map, &images](string sn)
+	for(int camera_idx=0; camera_idx<CAMERAS; camera_idx++)
 	{
+		string sn = serial_numbers[camera_idx];
 		string file_name = "../Common/Image/IR/" + sn + ".png";
 		Mat image = imread(file_name);
 		images[sn] = image;
@@ -98,16 +136,19 @@ int main()
 
 			for (int i = 0; i < ids.size(); i++)
 			{
-				if (sn == serial_numbers[1])
+				if (sn == serial_numbers[0])
 				{
-					for (int j = 0; j < 4; j++)
-					{
-						image_points.push_back(corners[indices[i]][j]);
-					}
+					//vector<Point3d> corners3D = getCornersInCameraWorld(MARKER_SIDE, rvecs[indices[i]], tvecs[indices[i]]);
+					//object_points.insert(object_points.end(), corners3D.begin(), corners3D.end());
+				}
+
+				for (int j = 0; j < 4; j++)
+				{
+					image_points[camera_idx].push_back(corners[indices[i]][j]);
 				}
 			}
 		}
-	});
+	}
 
 	Mat camera_rvec, camera_rot, camera_tvec;
 
@@ -118,10 +159,7 @@ int main()
 		return -1;
 	}
 
-	fs2["R1"] >> camera_rvec;
-	fs2["t1"] >> camera_tvec;
-
-	fs2.release();
+	
 
 	vector<Point3d> object_points;
 	FILE* fptr = fopen("../Common/Correspondence/test2/point3d.txt", "r");
@@ -131,36 +169,45 @@ int main()
 		exit(1);
 	};
 
-	int num_points;
-	fscanf(fptr, "%d", &num_points);
+	int num_points_all;
+	fscanf(fptr, "%d", &num_points_all);
+	int num_points_per_camera = num_points_all / 2;
 
-	for (int i = 0; i < num_points; i++)
+	for (int camera_idx = 0; camera_idx < CAMERAS; camera_idx++)
 	{
-		Point3d point;
-		fscanf(fptr, "%lf", &point.x);
-		fscanf(fptr, "%lf", &point.y);
-		fscanf(fptr, "%lf", &point.z);
-		object_points.emplace_back(point);
-	}
-	fclose(fptr);
+		object_points.clear();
 
+		for (int i = 0; i < num_points_per_camera; i++)
+		{
+			Point3d point;
+			fscanf(fptr, "%lf", &point.x);
+			fscanf(fptr, "%lf", &point.y);
+			fscanf(fptr, "%lf", &point.z);
+			object_points.emplace_back(point);
+		}
 
-	vector<Point2d> reprojected_points;
-	projectPoints(object_points, camera_rvec, camera_tvec, camera_matrix_map[serial_numbers[1]], dist_coeffs_map[serial_numbers[1]], reprojected_points);
+		fs2["R" + to_string(camera_idx)] >> camera_rvec;
+		fs2["t" + to_string(camera_idx)] >> camera_tvec;
 
-	//visualization
-	Mat reprojection_image = images[serial_numbers[1]];
-	for (int i = 0; i < reprojected_points.size(); i++)
-	{
-		drawMarker(reprojection_image, image_points[i], Scalar(255, 0, 0), MARKER_CROSS, 10, 2);
-		drawMarker(reprojection_image, reprojected_points[i], Scalar(0, 255, 0), MARKER_CROSS, 10, 2);
-	}
+		vector<Point2d> reprojected_points;
+		projectPoints(object_points, camera_rvec, camera_tvec, camera_matrix_map[serial_numbers[camera_idx]], dist_coeffs_map[serial_numbers[camera_idx]], reprojected_points);
 
-	putText(reprojection_image, "BLUE : Marker Points (t+1)", Point{ 10,20 }, FONT_HERSHEY_SIMPLEX, 0.7, Scalar(255, 0, 0), 2);
-	putText(reprojection_image, "GREEN : Reprojected Points (t -> t+1)", Point{ 10,45 }, FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0, 255, 0), 2);
+		//visualization
+		Mat reprojection_image = images[serial_numbers[camera_idx]];
+		for (int i = 0; i < reprojected_points.size(); i++)
+		{
+			drawMarker(reprojection_image, image_points[camera_idx][i], Scalar(255, 0, 0), MARKER_CROSS, 10, 2);
+			drawMarker(reprojection_image, reprojected_points[i], Scalar(0, 255, 0), MARKER_CROSS, 10, 2);
+		}
 
-	imshow("Reprojection", reprojection_image);
+		putText(reprojection_image, "BLUE : Marker Points (t+1)", Point{ 10,20 }, FONT_HERSHEY_SIMPLEX, 0.7, Scalar(255, 0, 0), 2);
+		putText(reprojection_image, "GREEN : Reprojected Points (t -> t+1)", Point{ 10,45 }, FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0, 255, 0), 2);
 
+		imshow("Reprojection: " + serial_numbers[camera_idx], reprojection_image);
+	}
+
+	fclose(fptr);
+	fs2.release();
 
 	while (true)
 	{

Original file line number	Diff line number	Diff line change
`@@ -71,7 +71,7 @@ class BALProblem`
`71`	`71`
`72`	`72`	`void getPoint3dCoordinates(vector<Point3d>& points)`
`73`	`73`	`{`
`74`		`- for (int i = num_base_camera_observations(); i < num_observations(); i++)`
	`74`	`+ for (int i = 0; i < num_observations(); i++)`
`75`	`75`	`{`
`76`	`76`	`double* base_marker_transform = mutable_base_marker_transform_from_base_camera(i);`
`77`	`77`	`double* marker_transform = mutable_marker_transform_from_base_marker(i);`