Add uncertainty params, gui layout changes

* Introduced angular and distance uncertainty in gui and config files,
used for matching features, and stored in model
* Rearranged gui elements to better match with calib workflow - like editing
initial calib values along with the calib params
* Moved calib params object to core classes
* Stored results of solver in separate object, using for printing
* Code cleaning

config_files/app_config_checkerboard.ini

@@ -4,11 +4,17 @@
 path=/home/karnik/dataset/checkerboard.rawlog
 
 [initial_calibration]
-#transformation matrix for first sensor in the rawlog
-RGBD_1=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1] //sensor_label=[4x4 matrix]
+#transformation matrices for the sensors in the rawlog
 
-#transformation matrix for second sensor in the rawlog
-RGBD_2=[0.7071 0 0.7071 0; 0 1 0 0; -0.701 0 0.7071 0; 0 0 0 1]
+RGBD_1=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]
+RGBD_2=[0.7071 0 0.7071 0; 0 1 0 0; -0.7071 0 0.7071 0; 0 0 0 1]
+
+[initial_uncertainty]
+#The angular and distance uncertainty of each sensor in the rawlog
+#Units are degrees and meters
+
+RGBD_1=[10 0.1]
+RGBD_2=[10 0.1]
 
 [grouping_observations]
 #maximum acceptable delay between observations in milliseconds
@@ -36,20 +42,12 @@ max_cos_normal=0.998
 dist_centre_plane_threshold=0.1 //min distance between center points of planes
 proximity_threshold=0.4 //min distance between the closest points of planes
 
-[plane_matching]
-min_normals_dot_product=0.9
-max_plane_dist_diff=0.2
-
 [line_segmentation]
 canny_low_threshold=150
 canny_high_to_low_ratio=3
 canny_kernel_size=3
 hough_threshold=150
 
-[line_matching]
-min_normals_dot_product=0.9
-max_line_normal_dot_product=0.1
-
 [solver]
 max_iters=10
 min_update=0.00001

config_files/app_config_livingroom.ini

@@ -4,13 +4,22 @@
 path=/home/karnik/dataset/livingroom.rawlog
 
 [initial_calibration]
-RGBD_1=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1] //sensor_label=[4x4 matrix]
+#transformation matrices for the sensors in the rawlog
 
-RGBD_2=[0.7071 0 0.7071 0; 0 1 0 0; -0.701 0 0.7071 0; 0 0 0 1]
+RGBD_1=[0.70711 0 0.70711 0.27100; -0.70711 0 0.70711 -0.03500; 0 -1 0 1.0150; 0 0 0 1]
+RGBD_2=[0 0 1 0.2400; -1 0 0 -0.0490; 0 -1 0 1.0150; 0 0 0 1]
+RGBD_3=[0.70711 0 -0.70711 0.27100; 0.70711 0 0.70711 0.02700; 0 -1 0 1.0150; 0 0 0 1]
+RGBD_4=[1 0 0 0.28500; 0 0 1 -0.0400; 0 -1 0 1.0150; 0 0 0 1]
 
-RGBD_3=[0.7071 0 0.7071 0; 0 1 0 0; -0.701 0 0.7071 0; 0 0 0 1]
+[initial_uncertainty]
 
-RGBD_4=[0.7071 0 0.7071 0; 0 1 0 0; -0.701 0 0.7071 0; 0 0 0 1]
+#The initial angular and distance uncertainty of each sensor in the rawlog
+#Units are degrees and meters
+
+RGBD_1=[10 0.1]
+RGBD_2=[10 0.1]
+RGBD_3=[10 0.1]
+RGBD_4=[10 0.1]
 
 [grouping_observations]
 #maximum acceptable delay between observations in milliseconds
@@ -38,21 +47,14 @@ max_cos_normal=0.998
 dist_centre_plane_threshold=0.1 //min distance between center points of planes
 proximity_threshold=0.4 //min distance between the closest points of planes
 
-[plane_matching]
-min_normals_dot_product=0.9
-max_plane_dist_diff=0.2
-
 [line_segmentation]
 canny_low_threshold=150
 canny_high_to_low_ratio=3
 canny_kernel_size=3
 hough_threshold=150
 
-[line_matching]
-min_normals_dot_product=0.9
-max_line_normal_dot_product=0.1
-
 [solver]
 max_iters=10
 min_update=0.00001
 convergence_error=0.00001
+

core/CMakeLists.txt

@@ -17,6 +17,7 @@ SET(SRC
 	calib_solvers/TCalibFromPlanesParams.h
 	calib_solvers/TCalibFromLinesParams.h
 	calib_solvers/TExtrinsicCalibParams.h
+	calib_solvers/TSolverResult.h
 
 	CObservationTree.cpp
 	CObservationTreeItem.cpp

core/CObservationTree.cpp

@@ -70,10 +70,13 @@ void CObservationTree::loadTree()
 	}
 
 	Eigen::Matrix4f rt;
+	Eigen::Vector2f uncertain;
 	for(size_t i = 0; i < m_sensor_labels.size(); i++)
 	{
 		m_config_file.read_matrix("initial_calibration", m_sensor_labels[i], rt, Eigen::Matrix4f(), true);
+		m_config_file.read_vector("initial_uncertainty", m_sensor_labels[i], Eigen::Vector2f(), uncertain, true);
 		m_sensor_poses.push_back(rt);
+		m_sensor_pose_uncertainties.push_back(uncertain);
 	}
 }
 
@@ -191,10 +194,13 @@ void CObservationTree::syncObservations(const std::vector<std::string> &selected
 	m_sync_offset = max_delay;
 
 	Eigen::Matrix4f rt;
+	Eigen::Vector2f uncertain;
 	for(size_t i = 0; i < m_sensor_labels.size(); i++)
 	{
 		m_config_file.read_matrix("initial_calibration", m_sensor_labels[i], rt, Eigen::Matrix4f(), true);
+		m_config_file.read_vector("initial_uncertainty", m_sensor_labels[i], Eigen::Vector2f(), uncertain, true);
 		m_sensor_poses.push_back(rt);
+		m_sensor_pose_uncertainties.push_back(uncertain);
 	}
 }
 
@@ -233,6 +239,16 @@ std::vector<Eigen::Matrix4f> CObservationTree::getSensorPoses() const
 	return this->m_sensor_poses;
 }
 
+std::vector<Eigen::Vector2f> CObservationTree::getSensorUncertainties() const
+{
+	return this->m_sensor_pose_uncertainties;
+}
+
+void CObservationTree::setSensorPose(const Eigen::Matrix4f &sensor_pose, const int &sensor_index)
+{
+	this->m_sensor_poses[sensor_index] = sensor_pose;
+}
+
 bool CObservationTree::setSensorPoses(const std::vector<Eigen::Matrix4f> &sensor_poses)
 {
 	if(sensor_poses.size() != this->m_sensor_labels.size())
@@ -241,6 +257,19 @@ bool CObservationTree::setSensorPoses(const std::vector<Eigen::Matrix4f> &sensor
 		this->m_sensor_poses = sensor_poses;
 }
 
+void CObservationTree::setSensorUncertainty(const Eigen::Vector2f &sensor_uncertainty, const int &sensor_index)
+{
+	this->m_sensor_pose_uncertainties[sensor_index] = sensor_uncertainty;
+}
+
+bool CObservationTree::setSensorUncertainties(const std::vector<Eigen::Vector2f> &uncertainties)
+{
+	if(uncertainties.size() != this->m_sensor_pose_uncertainties.size())
+		return 0;
+	else
+		this->m_sensor_pose_uncertainties = uncertainties;
+}
+
 std::vector<std::vector<int>> CObservationTree::getSyncIndices() const
 {
 	return this->m_sync_indices;

core/CObservationTree.h

@@ -60,12 +60,33 @@ class CObservationTree
 		/** Returns the poses of the sensors found in the rawlog. */
 		std::vector<Eigen::Matrix4f> getSensorPoses() const;
 
+		/** Returns the uncertainties of the sensors found in the rawlog. */
+		std::vector<Eigen::Vector2f> getSensorUncertainties() const;
+
 		/** Sets the poses of the sensors found in the rawlog.
-		 * \param the new sensor poses (size should match the number of sensors in the model).
+		 * \param sensor_poses the new sensor poses (size should match the number of sensors in the model).
 		 * \return true or false depending on whether the poses were set.
 		 */
 		bool setSensorPoses(const std::vector<Eigen::Matrix4f> &sensor_poses);
 
+		/** Sets the pose of an individual sensor in the rawlog.
+		 * \param sensor_pose the pose of the sensor.
+		 * \param sensor_index the index of the sensor in the rawlog.
+		 */
+		void setSensorPose(const Eigen::Matrix4f &sensor_pose, const int &sensor_index);
+
+		/** Sets the uncertainties of the sensors found in the rawlog.
+		 * \param uncertainties The new uncertainites of the sensors(size should match the number of sensors in the model).
+		 * \return bool depending on whether the uncertainties were set.
+		 */
+		bool setSensorUncertainties(const std::vector<Eigen::Vector2f> &uncertainties);
+
+		/** Set the distance and angular uncertainty of an individual sensor in the rawlog.
+		 * \param sensor_uncertainty The distance and angular uncertainty.
+		 * \param sensor_index The index of the sensor in the rawlog.
+		 */
+		void setSensorUncertainty(const Eigen::Vector2f &sensor_uncertainty, const int &sensor_index);
+
 		/** Groups observations together based on their time stamp proximity.
 		 * Stores the results back in the same tree.
 		 * \param the labels of the sensors that are to be considered for grouping.
@@ -109,4 +130,7 @@ class CObservationTree
 
 		/** The [R,t] poses of the sensors found in the rawlog. */
 		std::vector<Eigen::Matrix4f> m_sensor_poses;
+
+		/** The initial angular and distance uncertainties in the sensor poses. */
+		std::vector<Eigen::Vector2f> m_sensor_pose_uncertainties;
 };

core/Utils.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <iostream>
 #include <algorithm>
 #include <vector>
 #include <mrpt/img/TCamera.h>
@@ -27,22 +28,36 @@ namespace utils
 			return -1;
 	}
 
-	/** Function template to get so(3) rotation from SE(3) transformation. */
+	/** Function template to get rotation angles from SE(3) transformation. */
 	template <typename T>
-	Eigen::Matrix<T,3,1> getRotationVector(const Eigen::Matrix<T,4,4> &sensor_pose)
+	Eigen::Matrix<T,3,1> getRotations(const Eigen::Matrix<T,4,4> &sensor_pose)
 	{
 		Eigen::Matrix<T,3,1> rot_vec;
 		mrpt::math::CMatrixDouble44 mrpt_mat(sensor_pose);
 		mrpt::poses::CPose3D mrpt_pose(mrpt_mat);
-		mrpt::math::CArrayDouble<3> rot_vec_mrpt = mrpt_pose.ln_rotation();
-		rot_vec = Eigen::Matrix<T,3,1>(rot_vec_mrpt[0], rot_vec_mrpt[1], rot_vec_mrpt[2]);
-
+		rot_vec[0] = (mrpt_pose.roll() * 180) / M_PI;
+		rot_vec[1] = (mrpt_pose.pitch() * 180) / M_PI;
+		rot_vec[2] = (mrpt_pose.yaw() * 180) / M_PI;
 		return rot_vec;
 	}
 
+	/** Function template to convert rotation angles to SO(3). */
+	template<typename T>
+	Eigen::Matrix<T,3,3> getRotationMatrix(const Eigen::Matrix<T,3,1> &angles)
+	{
+		Eigen::Matrix<T,3,3> r, rx, ry, rz;
+
+		rx << 1, 0, 0, 0, cos(angles(0)), -1 * sin(angles(0)), 0, sin(angles(0)), cos(angles(0));
+		ry << cos(angles(1)), 0, sin(angles(1)), 0, 1, 0, -1 * sin(angles(1)), 0, cos(angles(1));
+		rz << cos(angles(2)), -1 * sin(angles(2)), 0, sin(angles(2)), cos(angles(2)), 0, 0, 0, 1;
+
+		r = rz * ry * rx;
+		return r;
+	}
+
 	/** Function template to get translation from SE(3) transformation. */
 	template <typename T>
-	Eigen::Matrix<T,3,1> getTranslationVector(const Eigen::Matrix<T,4,4> &sensor_pose)
+	Eigen::Matrix<T,3,1> getTranslations(const Eigen::Matrix<T,4,4> &sensor_pose)
 	{
 		return sensor_pose.block(0,3,3,1);
 	}

core/calib_solvers/CCalibFromLines.cpp

@@ -1,20 +1,23 @@
 #include "CCalibFromLines.h"
 #include <mrpt/math/geometry.h>
 
-CCalibFromLines::CCalibFromLines(CObservationTree *model) : CExtrinsicCalib(model)
-{}
+CCalibFromLines::CCalibFromLines(CObservationTree *model, TCalibFromLinesParams *params) :
+    CExtrinsicCalib(model)
+{
+	this->params = params;
+}
 
 CCalibFromLines::~CCalibFromLines(){}
 
-void CCalibFromLines::segmentLines(const cv::Mat &image, Eigen::MatrixXf &range, const TLineSegmentationParams &params, const mrpt::img::TCamera &camera_params, std::vector<CLine> &lines)
+void CCalibFromLines::segmentLines(const cv::Mat &image, Eigen::MatrixXf &range, const mrpt::img::TCamera &camera_params, std::vector<CLine> &lines)
 {
 	cv::Mat canny_image;
 
-	cv::Canny(image, canny_image, params.clow_threshold, params.clow_threshold * params.chigh_to_low_ratio, params.ckernel_size);
+	cv::Canny(image, canny_image, params->seg.clow_threshold, params->seg.clow_threshold * params->seg.chigh_to_low_ratio, params->seg.ckernel_size);
 
 	std::vector<cv::Vec2f> hlines;
 
-	cv::HoughLines(canny_image, hlines, 1, CV_PI, params.hthreshold);
+	cv::HoughLines(canny_image, hlines, 1, CV_PI, params->seg.hthreshold);
 
 
 	double rho, theta;
@@ -168,7 +171,7 @@ void CCalibFromLines::segmentLines(const cv::Mat &image, Eigen::MatrixXf &range,
 				if (canny_image.at<char>(y, x) == 0 || i == longest)
 				{
 					onSegment = false;
-					if (nbPixels >= params.hthreshold)
+					if (nbPixels >= params->seg.hthreshold)
 					{
 						std::array<Eigen::Vector2i, 2> end_points;
 						end_points[0] = Eigen::Vector2i(memoryX, memoryY);
@@ -244,49 +247,49 @@ void CCalibFromLines::segmentLines(const cv::Mat &image, Eigen::MatrixXf &range,
 	}
 }
 
-void CCalibFromLines::findPotentialMatches(const std::vector<std::vector<CLine>> &lines, const int &set_id, const TLineMatchingParams &params)
+void CCalibFromLines::findPotentialMatches(const std::vector<std::vector<CLine>> &lines, const int &set_id)
 {
+	std::vector<Eigen::Vector2f> sensor_pose_uncertainties = sync_model->getSensorUncertainties();
+
 	for(int i = 0; i < lines.size()-1; ++i)
 		for(int j = i+1; j < lines.size(); ++j)
 		{
 			for(int ii = 0; ii < lines[i].size(); ++ii)
-			{	for(int jj = 0; jj < lines[j].size(); ++jj)
+			{
+				for(int jj = 0; jj < lines[j].size(); ++jj)
 				{
 					Eigen::Vector3f n_ii = sync_model->getSensorPoses()[i].block(0,0,3,3) * lines[i][ii].normal;
 					Eigen::Vector3f v_ii = sync_model->getSensorPoses()[i].block(0,0,3,3) * lines[i][ii].v;
 
 					Eigen::Vector3f n_jj = sync_model->getSensorPoses()[j].block(0,0,3,3) * lines[j][jj].normal;
 					Eigen::Vector3f v_jj = sync_model->getSensorPoses()[j].block(0,0,3,3) * lines[j][jj].v;
 
-					if((n_ii.dot(n_jj) > params.min_normals_dot_prod) && (n_ii.dot(v_jj) < params.max_line_normal_dot_prod))
+					if((n_ii.dot(n_jj) > cos(sensor_pose_uncertainties[j](0) * (M_PI/180))) && (n_ii.dot(v_jj) < cos((90 - sensor_pose_uncertainties[j](0)) * (M_PI/180))))
 					{
 						std::array<int,3> potential_match{set_id, ii, jj};
 						mmv_line_corresp[i][j].push_back(potential_match);
 					}
 				}
 			}
 
-			// for stats printing when no matches exist
-			if((lines[i].size() == 0) || (lines[j].size() == 0))
-			{
-				std::array<int,3> temp_match{-1, -1, -1};
-				mmv_line_corresp[i][j].push_back(temp_match);
-			}
+//			for stats printing when no matches exist
+//			if((lines[i].size() == 0) || (lines[j].size() == 0))
+//			{
+//				std::array<int,3> temp_match{-1, -1, -1};
+//				mmv_line_corresp[i][j].push_back(temp_match);
+//			}
 		}
 }
 
-Scalar CCalibFromLines::computeRotationResidual(const std::vector<Eigen::Matrix4f> &sensor_poses)
+Scalar CCalibFromLines::computeRotationResidual()
 {
-
 }
 
-Scalar CCalibFromLines::computeRotation(const TSolverParams &params, const std::vector<Eigen::Matrix4f> & sensor_poses, std::string &stats)
+Scalar CCalibFromLines::computeRotation()
 {
-
 }
 
-Scalar CCalibFromLines::computeTranslation(const std::vector<Eigen::Matrix4f> &sensor_poses, std::string &stats)
+Scalar CCalibFromLines::computeTranslation()
 {
-
 }