Change use of camera matrix

applications/vision/src/main.cpp

@@ -58,7 +58,7 @@ static void run(const string &file) {
 	Universe net(config["net"]);
 	LOG(INFO) << "Net started.";
 
-	RGBDSource *source = nullptr;
+	StereoVideoSource *source = nullptr;
 	source = new StereoVideoSource(config, file);
 	
 	// Allow remote users to access camera calibration matrix
@@ -143,6 +143,7 @@ static void run(const string &file) {
 
 		// Send RGB+Depth images for local rendering
 		if (prgb.rows > 0) display.render(prgb, pdepth, source->getParameters());
+		if (config["display"]["right"]) cv::imshow("Right: ", source->getRight());
 		display.wait(1);
 
 		// Wait for both pipelines to complete

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

@@ -29,6 +29,8 @@ class StereoVideoSource : public RGBDSource {
 	void getRGBD(cv::Mat &rgb, cv::Mat &depth);
 	bool isReady();
 
+	const cv::Mat &getRight() const { return right_; }
+
 	static inline RGBDSource *create(nlohmann::json &config, ftl::net::Universe *net) {
 		return new StereoVideoSource(config, net);
 	}

components/rgbd-sources/src/calibrate.cpp

@@ -274,9 +274,15 @@ bool Calibrate::_loadCalibration() {
     		map1_[1], map2_[1]);
 
 	// Re-distort
-	initUndistortRectifyMap(M1, Mat(), R1.t(), P1,
+	Mat P1_cam = (cv::Mat_<double>(3,3) << P1.at<double>(0, 0), P1.at<double>(0, 1) , P1.at<double>(0, 2),
+		P1.at<double>(1, 0), P1.at<double>(1, 1), P1.at<double>(1, 2),
+		P1.at<double>(2, 0), P1.at<double>(2, 1), P1.at<double>(2, 2));
+	Mat M1_trans = (cv::Mat_<double>(3, 4) << M1.at<double>(0, 0), M1.at<double>(0, 1), M1.at<double>(0, 2), -P1.at<double>(0, 3),
+		M1.at<double>(1, 0), M1.at<double>(1, 1), M1.at<double>(1, 2), -P1.at<double>(1, 3),
+		M1.at<double>(2, 0), M1.at<double>(2, 1), M1.at<double>(2, 2), -P1.at<double>(2, 3));
+	initUndistortRectifyMap(P1_cam, Mat(), R1.t(), P1,
 			img_size, CV_16SC2, imap1_, imap2_);
-	r1_ = R1.t();
+	r1_ = P1;
     return true;
 }
 

components/rgbd-sources/src/calibrate.hpp

@@ -108,6 +108,7 @@ class Calibrate {
 	 * a 3D point cloud.
 	 */
 	const cv::Mat &getQ() const { return Q_; }
+	const cv::Mat &getCameraMatrix() const { return r1_; }
 
 	private:
 	bool _recalibrate(std::vector<std::vector<cv::Point2f>> *imagePoints,

components/rgbd-sources/src/stereovideo_source.cpp

@@ -46,11 +46,12 @@ StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file)
 	else LOG(INFO) << "Calibration initiated.";
 
 	// Generate camera parameters from Q matrix
-	cv::Mat q = calib_->getQ();
+	cv::Mat q = calib_->getCameraMatrix();
 	params_ = {
-		q.at<double>(2,3),	// F
-		q.at<double>(0,3),	// Cx
-		q.at<double>(1,3),	// Cy
+		// TODO(Nick) Add fx and fy
+		q.at<double>(0,0),	// F
+		q.at<double>(0,2),	// Cx
+		q.at<double>(1,2),	// Cy
 		(unsigned int)left_.cols,  // TODO (Nick)
 		(unsigned int)left_.rows,
 		0.0f,	// 0m min
@@ -106,5 +107,5 @@ void StereoVideoSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
 	disp_->compute(left_, right_, disp);
 	rgb = left_;
 	disparityToDepth(disp, depth, calib_->getQ());
-	calib_->distort(rgb,depth);
+	//calib_->distort(rgb,depth);
 }