diff --git a/config_files/app_config_checkerboard.ini b/config_files/app_config_checkerboard.ini index 388677d..6d81651 100644 --- a/config_files/app_config_checkerboard.ini +++ b/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 diff --git a/config_files/app_config_livingroom.ini b/config_files/app_config_livingroom.ini index 4f9dce4..b4ec437 100755 --- a/config_files/app_config_livingroom.ini +++ b/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 + diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 2060fe2..50ee8ea 100755 --- a/core/CMakeLists.txt +++ b/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 diff --git a/core/CObservationTree.cpp b/core/CObservationTree.cpp index 64d6a1f..53a9a02 100644 --- a/core/CObservationTree.cpp +++ b/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 &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 CObservationTree::getSensorPoses() const return this->m_sensor_poses; } +std::vector 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 &sensor_poses) { if(sensor_poses.size() != this->m_sensor_labels.size()) @@ -241,6 +257,19 @@ bool CObservationTree::setSensorPoses(const std::vector &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 &uncertainties) +{ + if(uncertainties.size() != this->m_sensor_pose_uncertainties.size()) + return 0; + else + this->m_sensor_pose_uncertainties = uncertainties; +} + std::vector> CObservationTree::getSyncIndices() const { return this->m_sync_indices; diff --git a/core/CObservationTree.h b/core/CObservationTree.h index 74cb286..9323938 100644 --- a/core/CObservationTree.h +++ b/core/CObservationTree.h @@ -60,12 +60,33 @@ class CObservationTree /** Returns the poses of the sensors found in the rawlog. */ std::vector getSensorPoses() const; + /** Returns the uncertainties of the sensors found in the rawlog. */ + std::vector 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 &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 &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 m_sensor_poses; + + /** The initial angular and distance uncertainties in the sensor poses. */ + std::vector m_sensor_pose_uncertainties; }; diff --git a/core/Utils.h b/core/Utils.h index 2f5f17d..e931684 100644 --- a/core/Utils.h +++ b/core/Utils.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -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 - Eigen::Matrix getRotationVector(const Eigen::Matrix &sensor_pose) + Eigen::Matrix getRotations(const Eigen::Matrix &sensor_pose) { Eigen::Matrix 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(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 + Eigen::Matrix getRotationMatrix(const Eigen::Matrix &angles) + { + Eigen::Matrix 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 - Eigen::Matrix getTranslationVector(const Eigen::Matrix &sensor_pose) + Eigen::Matrix getTranslations(const Eigen::Matrix &sensor_pose) { return sensor_pose.block(0,3,3,1); } diff --git a/core/calib_solvers/CCalibFromLines.cpp b/core/calib_solvers/CCalibFromLines.cpp index e84f51a..6aa8b0b 100755 --- a/core/calib_solvers/CCalibFromLines.cpp +++ b/core/calib_solvers/CCalibFromLines.cpp @@ -1,20 +1,23 @@ #include "CCalibFromLines.h" #include -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 ¶ms, const mrpt::img::TCamera &camera_params, std::vector &lines) +void CCalibFromLines::segmentLines(const cv::Mat &image, Eigen::MatrixXf &range, const mrpt::img::TCamera &camera_params, std::vector &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 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(y, x) == 0 || i == longest) { onSegment = false; - if (nbPixels >= params.hthreshold) + if (nbPixels >= params->seg.hthreshold) { std::array end_points; end_points[0] = Eigen::Vector2i(memoryX, memoryY); @@ -244,13 +247,16 @@ void CCalibFromLines::segmentLines(const cv::Mat &image, Eigen::MatrixXf &range, } } -void CCalibFromLines::findPotentialMatches(const std::vector> &lines, const int &set_id, const TLineMatchingParams ¶ms) +void CCalibFromLines::findPotentialMatches(const std::vector> &lines, const int &set_id) { + std::vector 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; @@ -258,7 +264,7 @@ void CCalibFromLines::findPotentialMatches(const std::vector> 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 potential_match{set_id, ii, jj}; mmv_line_corresp[i][j].push_back(potential_match); @@ -266,27 +272,24 @@ void CCalibFromLines::findPotentialMatches(const std::vector> } } - // for stats printing when no matches exist - if((lines[i].size() == 0) || (lines[j].size() == 0)) - { - std::array 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 temp_match{-1, -1, -1}; +// mmv_line_corresp[i][j].push_back(temp_match); +// } } } -Scalar CCalibFromLines::computeRotationResidual(const std::vector &sensor_poses) +Scalar CCalibFromLines::computeRotationResidual() { - } -Scalar CCalibFromLines::computeRotation(const TSolverParams ¶ms, const std::vector & sensor_poses, std::string &stats) +Scalar CCalibFromLines::computeRotation() { - } -Scalar CCalibFromLines::computeTranslation(const std::vector &sensor_poses, std::string &stats) +Scalar CCalibFromLines::computeTranslation() { - } diff --git a/core/calib_solvers/CCalibFromLines.h b/core/calib_solvers/CCalibFromLines.h index c145ad9..5ff3eea 100755 --- a/core/calib_solvers/CCalibFromLines.h +++ b/core/calib_solvers/CCalibFromLines.h @@ -15,7 +15,10 @@ class CCalibFromLines : public CExtrinsicCalib { -public: +protected: + + /** The parameters for the calibration. */ + TCalibFromLinesParams *params; /** The segmented lines, the vector indices to access them are [sensor_id][obs_id][line_id] * obs_id is with respect to the synchronized model. @@ -28,7 +31,7 @@ class CCalibFromLines : public CExtrinsicCalib */ std::map>>> mmv_line_corresp; - CCalibFromLines(CObservationTree *model); + CCalibFromLines(CObservationTree *model, TCalibFromLinesParams *params); ~CCalibFromLines(); /** @@ -36,7 +39,7 @@ class CCalibFromLines : public CExtrinsicCalib * \param image the input image * \param lines vector of lines segmented */ - void segmentLines(const cv::Mat &image, Eigen::MatrixXf &range, const TLineSegmentationParams ¶ms, const mrpt::img::TCamera &camera_params, std::vector &lines); + void segmentLines(const cv::Mat &image, Eigen::MatrixXf &range, const mrpt::img::TCamera &camera_params, std::vector &lines); /** * Search for potential line matches between each sensor pair in a syc obs set. @@ -44,22 +47,19 @@ class CCalibFromLines : public CExtrinsicCalib * \param set_id the id of the synchronized set the lines belong to. * \param params the parameters for line matching. */ - void findPotentialMatches(const std::vector> &lines, const int &set_id, const TLineMatchingParams ¶ms); + void findPotentialMatches(const std::vector> &lines, const int &set_id); /** Calculate the angular residual error of the correspondences. - * \param sensor_poses relative poses of the sensors * \return the residual */ - virtual Scalar computeRotationResidual(const std::vector &sensor_poses); + virtual Scalar computeRotationResidual(); /** Compute Calibration (only rotation). - * \param sensor_poses initial calibration * \return the residual */ - virtual Scalar computeRotation(const TSolverParams ¶ms, const std::vector &sensor_poses, std::string &stats); + virtual Scalar computeRotation(); /** Compute Calibration (only translation). - \param sensor_poses initial calibration \return the residual */ - virtual Scalar computeTranslation(const std::vector &sensor_poses, std::string &stats); + virtual Scalar computeTranslation(); }; diff --git a/core/calib_solvers/CCalibFromPlanes.cpp b/core/calib_solvers/CCalibFromPlanes.cpp index 2a65e7f..ed7929c 100755 --- a/core/calib_solvers/CCalibFromPlanes.cpp +++ b/core/calib_solvers/CCalibFromPlanes.cpp @@ -22,7 +22,7 @@ using namespace std; -CCalibFromPlanes::CCalibFromPlanes(CObservationTree *model) : +CCalibFromPlanes::CCalibFromPlanes(CObservationTree *model, TCalibFromPlanesParams *params) : CExtrinsicCalib(model) { for(int sensor_id1=0; sensor_id1 < sync_model->getNumberOfSensors(); sensor_id1++) @@ -32,24 +32,26 @@ CCalibFromPlanes::CCalibFromPlanes(CObservationTree *model) : for(int sensor_id2 = sensor_id1 + 1; sensor_id2 < sync_model->getNumberOfSensors(); sensor_id2++) mmv_plane_corresp[sensor_id1][sensor_id2] = std::vector>(); } + + this->params = params; } -void CCalibFromPlanes::segmentPlanes(const pcl::PointCloud::Ptr &cloud, const TPlaneSegmentationParams & params, std::vector & planes) +void CCalibFromPlanes::segmentPlanes(const pcl::PointCloud::Ptr &cloud, std::vector & planes) { - unsigned min_inliers = params.min_inliers_frac * cloud->size(); + unsigned min_inliers = params->seg.min_inliers_frac * cloud->size(); pcl::IntegralImageNormalEstimation normal_estimation; - if(params.normal_estimation_method == 0) + if(params->seg.normal_estimation_method == 0) normal_estimation.setNormalEstimationMethod(normal_estimation.COVARIANCE_MATRIX); - else if(params.normal_estimation_method == 1) + else if(params->seg.normal_estimation_method == 1) normal_estimation.setNormalEstimationMethod(normal_estimation.AVERAGE_3D_GRADIENT); else normal_estimation.setNormalEstimationMethod(normal_estimation.AVERAGE_DEPTH_CHANGE); - normal_estimation.setDepthDependentSmoothing(params.depth_dependent_smoothing); - normal_estimation.setMaxDepthChangeFactor(params.max_depth_change_factor); - normal_estimation.setNormalSmoothingSize(params.normal_smoothing_size); + normal_estimation.setDepthDependentSmoothing(params->seg.depth_dependent_smoothing); + normal_estimation.setMaxDepthChangeFactor(params->seg.max_depth_change_factor); + normal_estimation.setNormalSmoothingSize(params->seg.normal_smoothing_size); pcl::PointCloud::Ptr normal_cloud(new pcl::PointCloud); normal_estimation.setInputCloud(cloud); @@ -57,8 +59,8 @@ void CCalibFromPlanes::segmentPlanes(const pcl::PointCloud::P pcl::OrganizedMultiPlaneSegmentation multi_plane_segmentation; multi_plane_segmentation.setMinInliers(min_inliers); - multi_plane_segmentation.setAngularThreshold(params.angle_threshold); - multi_plane_segmentation.setDistanceThreshold(params.dist_threshold); + multi_plane_segmentation.setAngularThreshold(params->seg.angle_threshold); + multi_plane_segmentation.setDistanceThreshold(params->seg.dist_threshold); multi_plane_segmentation.setInputNormals(normal_cloud); multi_plane_segmentation.setInputCloud(cloud); @@ -80,7 +82,7 @@ void CCalibFromPlanes::segmentPlanes(const pcl::PointCloud::P { CPlaneCHull planehull; - if(regions[i].getCurvature() > params.max_curvature) + if(regions[i].getCurvature() > params->seg.max_curvature) continue; mrpt::pbmap::Plane plane; @@ -113,7 +115,7 @@ void CCalibFromPlanes::segmentPlanes(const pcl::PointCloud::P bool isSamePlane = false; for (size_t j = 0; j < pbmap.vPlanes.size(); j++) - if(pbmap.vPlanes[j].isSamePlane(plane, params.max_cos_normal, params.dist_centre_plane_threshold, params.proximity_threshold)) + if(pbmap.vPlanes[j].isSamePlane(plane, params->seg.max_cos_normal, params->seg.dist_centre_plane_threshold, params->seg.proximity_threshold)) { isSamePlane = true; pbmap.vPlanes[j].mergePlane(plane); @@ -147,11 +149,14 @@ void CCalibFromPlanes::segmentPlanes(const pcl::PointCloud::P } } -void CCalibFromPlanes::findPotentialMatches(const std::vector> &planes, const int &set_id, const TPlaneMatchingParams ¶ms) +void CCalibFromPlanes::findPotentialMatches(const std::vector> &planes, const int &set_id) { + std::vector sensor_pose_uncertainties = sync_model->getSensorUncertainties(); + for(int i = 0; i < planes.size()-1; ++i) for(int j = i+1; j < planes.size(); ++j) - { for(int ii = 0; ii < planes[i].size(); ++ii) + { + for(int ii = 0; ii < planes[i].size(); ++ii) { for(int jj = 0; jj < planes[j].size(); ++jj) { @@ -159,7 +164,7 @@ void CCalibFromPlanes::findPotentialMatches(const std::vectorgetSensorPoses()[i].block(0,3,3,1)).dot(n_ii); Eigen::Vector3f n_jj = sync_model->getSensorPoses()[j].block(0,0,3,3) * planes[j][jj].v3normal; Scalar d2 = planes[j][jj].d - Eigen::Vector3f(sync_model->getSensorPoses()[j].block(0,3,3,1)).dot(n_jj); - if((n_ii.dot(n_jj) > params.min_normals_dot_prod) && (d1 - d2 < params.max_dist_diff)) + if((n_ii.dot(n_jj) > cos(sensor_pose_uncertainties[j][0] * (M_PI/180))) && (d1 - d2 < sensor_pose_uncertainties[j][1])) { std::array potential_match{set_id, ii, jj}; mmv_plane_corresp[i][j].push_back(potential_match); @@ -176,8 +181,9 @@ void CCalibFromPlanes::findPotentialMatches(const std::vector & sensor_poses) +Scalar CCalibFromPlanes::computeRotationResidual() { + std::vector sensor_poses = sync_model->getSensorPoses(); Scalar sum_squared_error = 0.; // Accumulated squared error for all plane correspondences for(std::map>>>::iterator it_sensor_i = mmv_plane_corresp.begin(); @@ -210,8 +216,9 @@ Scalar CCalibFromPlanes::computeRotationResidual(const std::vector & sensor_poses, std::string &stats) +Scalar CCalibFromPlanes::computeRotation() { + std::vector sensor_poses = sync_model->getSensorPoses(); const int num_sensors = sensor_poses.size(); const int dof = 3 * (num_sensors - 1); Eigen::VectorXf update_vector(dof); @@ -225,9 +232,9 @@ Scalar CCalibFromPlanes::computeRotation(const TSolverParams ¶ms, const std: float increment = 1000, diff_error = 1000; int it = 0; - init_error = computeRotationResidual(sensor_poses); + init_error = computeRotationResidual(); - while(it < params.max_iters && increment > params.min_update && diff_error > params.converge_error) + while(it < params->solver.max_iters && increment > params->solver.min_update && diff_error > params->solver.converge_error) { // Calculate the hessian and the gradient hessian = Eigen::Matrix::Zero(dof, dof); // Hessian of the rotation of the decoupled system @@ -292,7 +299,7 @@ Scalar CCalibFromPlanes::computeRotation(const TSolverParams ¶ms, const std: Eigen::FullPivLU> lu(hessian); if(!lu.isInvertible()) { - stats = "System is badly conditioned. Please try again with a new set of observations."; + result.msg = "System is badly conditioned. Please try again with a new set of observations."; break; } @@ -321,7 +328,7 @@ Scalar CCalibFromPlanes::computeRotation(const TSolverParams ¶ms, const std: //accum_error = computeRotationResidual(estimated_poses); //cout << "Error accumulated " << accum_error2 << endl; - new_error = computeRotationResidual(estimated_poses_temp); + new_error = computeRotationResidual(); //cout << "New rotation error " << new_accum_error2 << endl; //cout << "Closing loop? \n" << Rt_estimated[0].inverse() * Rt_estimated[7] * Rt_78; @@ -337,21 +344,24 @@ Scalar CCalibFromPlanes::computeRotation(const TSolverParams ¶ms, const std: //cout << "Iteration " << it << " increment " << increment << " diff_error " << diff_error << endl; } - std::stringstream stream; - for(int sensor_id = 0; sensor_id < num_sensors; sensor_id++) - stream << estimated_poses[sensor_id].block(0,0,3,3); + if(it == params->solver.max_iters) + result.msg = "Maximum iterations reached."; + else if(increment < params->solver.min_update) + result.msg = "Increment too small."; + else if(diff_error < params->solver.converge_error) + result.msg = "Convergence"; - stats += "Initial error: " + std::to_string(init_error); - stats += "\nNumber of iterations: " + std::to_string(it); - stats += "\nFinal error: " + std::to_string(new_error); - stats += "\n\nEstimated rotation: \n"; - stats += stream.str(); + result.init_error = init_error; + result.num_iters = it; + result.final_error = new_error; + result.estimate = estimated_poses; //std::cout << "ErrorCalibRotation " << accum_error2/numPlaneCorresp << " " << av_angle_error/numPlaneCorresp << std::endl; } -Scalar CCalibFromPlanes::computeTranslation(const std::vector & sensor_poses, std::string &stats) +Scalar CCalibFromPlanes::computeTranslation() { + std::vector sensor_poses = sync_model->getSensorPoses(); const int num_sensors = sensor_poses.size(); const int dof = 3 * (num_sensors - 1); Eigen::VectorXf update_vector(dof); @@ -415,7 +425,7 @@ Scalar CCalibFromPlanes::computeTranslation(const std::vector & Eigen::FullPivLU> lu(hessian); if(!lu.isInvertible()) { - stats = "System is badly conditioned. Please try again with a new set of observations."; + result.msg = "System is badly conditioned. Please try again with a new set of observations."; return accum_error2; } diff --git a/core/calib_solvers/CCalibFromPlanes.h b/core/calib_solvers/CCalibFromPlanes.h index 52e4140..bb193a3 100755 --- a/core/calib_solvers/CCalibFromPlanes.h +++ b/core/calib_solvers/CCalibFromPlanes.h @@ -10,6 +10,7 @@ #include "CExtrinsicCalib.h" #include "TCalibFromPlanesParams.h" +#include "TSolverResult.h" #include //#include //#include @@ -21,7 +22,13 @@ class CCalibFromPlanes : public CExtrinsicCalib { - public: + protected: + + /** The parameters for the calibration. */ + TCalibFromPlanesParams *params; + + /** Holds the result of the last solver run. */ + TSolverResult result; /** The segmented planes, the vector indices to access them are [sensor_id][obs_id][plane_id] * obs_id is with respect to the synchronized model. @@ -39,7 +46,7 @@ class CCalibFromPlanes : public CExtrinsicCalib std::vector< Eigen::Matrix, Eigen::aligned_allocator > > m_covariance_trans; /*! Constructor */ - CCalibFromPlanes(CObservationTree *model); + CCalibFromPlanes(CObservationTree *model, TCalibFromPlanesParams *params); /*! Destructor */ virtual ~CCalibFromPlanes(){} @@ -50,7 +57,7 @@ class CCalibFromPlanes : public CExtrinsicCalib * @param params the parameters for segmentation. * @param planes the segmented planes. */ - void segmentPlanes(const pcl::PointCloud::Ptr &cloud, const TPlaneSegmentationParams ¶ms, std::vector &planes); + void segmentPlanes(const pcl::PointCloud::Ptr &cloud, std::vector &planes); /** * Search for potential plane matches between each sensor pair in a sync obs set. @@ -58,7 +65,7 @@ class CCalibFromPlanes : public CExtrinsicCalib * \param set_id the id of the synchronized set the planes belong to. * \param params the parameters for plane matching. */ - void findPotentialMatches(const std::vector> &planes, const int &set_id, const TPlaneMatchingParams ¶ms); + void findPotentialMatches(const std::vector> &planes, const int &set_id); /** Calculate the residual error of the correspondences. \param sensor_poses relative poses of the sensors @@ -67,9 +74,8 @@ class CCalibFromPlanes : public CExtrinsicCalib // virtual Scalar computeCalibResidual(const std::vector > & sensor_poses); /** Calculate the angular residual error of the correspondences. - \param sensor_poses relative poses of the sensors \return the residual */ - virtual Scalar computeRotationResidual(const std::vector &sensor_poses); + virtual Scalar computeRotationResidual(); // /** Calculate the translational residual error of the correspondences. // \param sensor_poses relative poses of the sensors @@ -82,13 +88,10 @@ class CCalibFromPlanes : public CExtrinsicCalib // virtual Scalar computeCalibration(const std::vector > & sensor_poses); /** Compute Calibration (only rotation). - \param sensor_poses initial calibration \return the residual */ - virtual Scalar computeRotation(const TSolverParams ¶ms, const std::vector &sensor_poses, std::string &stats); + virtual Scalar computeRotation(); /** Compute Calibration (only translation). - \param sensor_poses initial calibration \return the residual */ - virtual Scalar computeTranslation(const std::vector &sensor_poses, std::string &stats); - + virtual Scalar computeTranslation(); }; diff --git a/core/calib_solvers/CExtrinsicCalib.cpp b/core/calib_solvers/CExtrinsicCalib.cpp index 8884c53..7ce7262 100755 --- a/core/calib_solvers/CExtrinsicCalib.cpp +++ b/core/calib_solvers/CExtrinsicCalib.cpp @@ -13,7 +13,7 @@ //Scalar CExtrinsicCalib::eigenvalue_ratio_threshold = 2e-4; double CExtrinsicCalib::eigenvalue_ratio_threshold = 2e-4; -Scalar CExtrinsicCalib::computeCalibration(const std::vector & sensor_poses) +Scalar CExtrinsicCalib::computeCalibration() { // std::string stats; // computeRotation(sensor_poses, stats); diff --git a/core/calib_solvers/CExtrinsicCalib.h b/core/calib_solvers/CExtrinsicCalib.h index 918bf65..86ae5fe 100755 --- a/core/calib_solvers/CExtrinsicCalib.h +++ b/core/calib_solvers/CExtrinsicCalib.h @@ -65,7 +65,7 @@ class CExtrinsicCalib /** Calculate the angular residual error of the correspondences. \param sensor_poses relative poses of the sensors \return the residual */ - virtual Scalar computeRotationResidual(const std::vector &sensor_poses) = 0; + virtual Scalar computeRotationResidual() = 0; // /** Calculate the translational residual error of the correspondences. // \param sensor_poses relative poses of the sensors @@ -75,18 +75,16 @@ class CExtrinsicCalib /** Compute Calibration. \param sensor_poses initial calibration \return the residual */ - virtual Scalar computeCalibration(const std::vector & sensor_poses); // = 0; + virtual Scalar computeCalibration(); // = 0; /** Compute Calibration (only rotation). * \params params the parameters related to the least-squares solver - * \param sensor_poses the initial calibration * \return the residual */ - virtual Scalar computeRotation(const TSolverParams ¶ms, const std::vector & sensor_poses, std::string &stats) = 0; + virtual Scalar computeRotation() = 0; /** Compute Calibration (only translation). - \param sensor_poses initial calibration \return the residual */ - virtual Scalar computeTranslation(const std::vector & sensor_poses, std::string &stats) = 0; + virtual Scalar computeTranslation() = 0; // /*! Compute the Fisher Information Matrix (FIM) of the rotation estimate. */ // TODO // void calcFIM_rot(); @@ -102,11 +100,8 @@ class CExtrinsicCalib /** The pose of the reference sensor wrt. the global reference system (i.e. the vehicle / robot platform). */ static mrpt::math::CMatrixFixedNumeric ref_sensor_pose; - /** The initial calibration (relative poses of the sensor systems wrt. the reference one) */ - static std::vector > m_init_calib; - /** The estimated calibration (relative poses of the sensor systems wrt. the reference one) */ - std::vector > m_calibration; + std::vector m_calibration; //std::vector > m_calibration(num_methods, Eigen::Affine3f::Identity()); /** The estimated calibration's uncertainty */ diff --git a/core/calib_solvers/TCalibFromLinesParams.h b/core/calib_solvers/TCalibFromLinesParams.h index 862bc5e..e219c91 100644 --- a/core/calib_solvers/TCalibFromLinesParams.h +++ b/core/calib_solvers/TCalibFromLinesParams.h @@ -15,18 +15,12 @@ struct TLineSegmentationParams int hthreshold; }; -struct TLineMatchingParams -{ - double min_normals_dot_prod; - double max_line_normal_dot_prod; -}; - /** * Maintains the status of the calibration progress. * This is useful when the calibration is run in steps and * the results of each step are to be visualized. */ -enum CalibrationFromLinesStatus +enum CalibFromLinesStatus { LCALIB_YET_TO_START, LINES_EXTRACTED, @@ -37,7 +31,6 @@ struct TCalibFromLinesParams { int downsample_factor; TLineSegmentationParams seg; - TLineMatchingParams match; TSolverParams solver; - CalibrationFromLinesStatus calib_status; + CalibFromLinesStatus calib_status; }; diff --git a/core/calib_solvers/TCalibFromPlanesParams.h b/core/calib_solvers/TCalibFromPlanesParams.h index 9d21374..1ec8763 100644 --- a/core/calib_solvers/TCalibFromPlanesParams.h +++ b/core/calib_solvers/TCalibFromPlanesParams.h @@ -24,18 +24,12 @@ struct TPlaneSegmentationParams double proximity_threshold; }; -struct TPlaneMatchingParams -{ - double min_normals_dot_prod; - double max_dist_diff; -}; - /** * Maintains the status of the calibration progress. * This is useful when the calibration is run in steps and * the results of each step are to be visualized. */ -enum CalibrationFromPlanesStatus +enum CalibFromPlanesStatus { PCALIB_YET_TO_START, PLANES_EXTRACTED, @@ -46,7 +40,6 @@ struct TCalibFromPlanesParams { int downsample_factor; TPlaneSegmentationParams seg; - TPlaneMatchingParams match; TSolverParams solver; - CalibrationFromPlanesStatus calib_status; + CalibFromPlanesStatus calib_status; }; diff --git a/core/calib_solvers/TSolverResult.h b/core/calib_solvers/TSolverResult.h new file mode 100644 index 0000000..2c3e2b9 --- /dev/null +++ b/core/calib_solvers/TSolverResult.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include + +/** Structure meant to hold the results of the calibration solver, useful for printing results. */ + +struct TSolverResult +{ + /** The number of iterations elapsed. */ + int num_iters; + + /** The error during initialization. */ + float init_error; + + /** The error with the estimated parameters. */ + float final_error; + + /** The estimated parameters. */ + std::vector estimate; + + /** Result message. */ + std::string msg; +}; diff --git a/gui/CMainWindow.cpp b/gui/CMainWindow.cpp index 756b593..c4d007f 100755 --- a/gui/CMainWindow.cpp +++ b/gui/CMainWindow.cpp @@ -42,6 +42,8 @@ CMainWindow::CMainWindow(QWidget *parent) : connect(m_ui->itx_sbox, SIGNAL(valueChanged(double)), this, SLOT(initCalibChanged(double))); connect(m_ui->ity_sbox, SIGNAL(valueChanged(double)), this, SLOT(initCalibChanged(double))); connect(m_ui->itz_sbox, SIGNAL(valueChanged(double)), this, SLOT(initCalibChanged(double))); + connect(m_ui->angle_uncertain_sbox, SIGNAL(valueChanged(double)), this, SLOT(initCalibChanged(double))); + connect(m_ui->distance_uncertain_sbox, SIGNAL(valueChanged(double)), this, SLOT(initCalibChanged(double))); setWindowTitle("Automatic Calibration of Sensor Extrinsics"); m_calib_from_planes_gui = nullptr; @@ -52,10 +54,10 @@ CMainWindow::CMainWindow(QWidget *parent) : m_recent_config_path = m_settings.value("recent_config").toString(); m_calib_from_planes_config_widget = std::make_shared(); - qobject_cast(m_ui->config_dockwidget_contents->layout())->insertWidget(3, m_calib_from_planes_config_widget.get()); + qobject_cast(m_ui->config_dockwidget_contents->layout())->insertWidget(8, m_calib_from_planes_config_widget.get()); m_calib_from_lines_config_widget = std::make_shared(); - qobject_cast(m_ui->config_dockwidget_contents->layout())->insertWidget(4, m_calib_from_lines_config_widget.get()); + qobject_cast(m_ui->config_dockwidget_contents->layout())->insertWidget(9, m_calib_from_lines_config_widget.get()); m_calib_from_planes_config_widget.get()->hide(); m_calib_from_lines_config_widget.get()->hide(); @@ -123,14 +125,6 @@ void CMainWindow::selectRawlogFile() void CMainWindow::loadRawlog() { // To ensure all options are disabled when a new rawlog is loaded again. - m_ui->sensor_cbox->setDisabled(true); - m_ui->irx_sbox->setDisabled(true); - m_ui->iry_sbox->setDisabled(true); - m_ui->irz_sbox->setDisabled(true); - m_ui->itx_sbox->setDisabled(true); - m_ui->ity_sbox->setDisabled(true); - m_ui->itz_sbox->setDisabled(true); - m_ui->med_sbox->setDisabled(true); m_ui->observations_treeview->setDisabled(true); m_ui->observations_description_textbrowser->setDisabled(true); m_ui->observations_delay_sbox->setDisabled(true); @@ -181,14 +175,6 @@ void CMainWindow::loadRawlog() m_ui->sensors_selection_list->setDisabled(false); m_ui->observations_delay_sbox->setDisabled(false); m_ui->sync_observations_button->setDisabled(false); - m_ui->sensor_cbox->setDisabled(false); - m_ui->irx_sbox->setDisabled(false); - m_ui->iry_sbox->setDisabled(false); - m_ui->irz_sbox->setDisabled(false); - m_ui->itx_sbox->setDisabled(false); - m_ui->ity_sbox->setDisabled(false); - m_ui->itz_sbox->setDisabled(false); - m_ui->med_sbox->setDisabled(false); m_ui->observations_delay_sbox->setValue(m_config_file.read_int("grouping_observations", "max_delay", 30, true)); @@ -201,7 +187,6 @@ void CMainWindow::loadRawlog() item->setFlags(item->flags() | Qt::ItemIsUserCheckable); item->setCheckState(Qt::Checked); m_ui->sensors_selection_list->insertItem(i, item); - m_ui->sensor_cbox->insertItem(i, QString::fromStdString(sensor_labels[i])); } std::string stats_string; @@ -222,14 +207,6 @@ void CMainWindow::loadRawlog() m_ui->viewer_container->updateText(stats_string); m_ui->viewer_container->updateSensorsList(m_model->getSensorLabels()); m_ui->viewer_container->updateSensorPoses(m_model->getSensorPoses()); - m_ui->sensor_cbox->setDisabled(false); - m_ui->irx_sbox->setDisabled(false); - m_ui->iry_sbox->setDisabled(false); - m_ui->irz_sbox->setDisabled(false); - m_ui->itx_sbox->setDisabled(false); - m_ui->ity_sbox->setDisabled(false); - m_ui->itz_sbox->setDisabled(false); - m_ui->med_sbox->setDisabled(false); } else @@ -283,20 +260,6 @@ void CMainWindow::listItemClicked(const QModelIndex &index) } } -void CMainWindow::sensorIndexChanged(int index) -{ -// Eigen::Matrix4f rt = m_model->getSensorPoses()[index]; -// Eigen::Matrix rvec = utils::getRotationVector(rt); -// Eigen::Matrix tvec = utils::getTranslationVector(rt); - -// m_ui->irx_sbox->setValue(rvec(0,0)); -// m_ui->iry_sbox->setValue(rvec(1,0)); -// m_ui->irz_sbox->setValue(rvec(2,0)); -// m_ui->itx_sbox->setValue(tvec(0,0)); -// m_ui->itx_sbox->setValue(tvec(1,0)); -// m_ui->itx_sbox->setValue(tvec(2,0)); -} - void CMainWindow::syncObservationsClicked() { std::vector selected_sensor_labels; @@ -304,11 +267,15 @@ void CMainWindow::syncObservationsClicked() m_ui->grouped_observations_treeview->setDisabled(true); m_ui->observations_treeview->setDisabled(false); - - //if(m_config_widget) - //m_config_widget.reset(); - //qobject_cast(m_ui->config_dockwidget_contents->layout())->removeWidget(m_config_widget.get()); - //if(qobject_cast(m_ui->config_dockwidget_contents->layout())->indexOf(m_config_widget.get()) != -1) + m_ui->sensor_cbox->setDisabled(true); + m_ui->irx_sbox->setDisabled(true); + m_ui->iry_sbox->setDisabled(true); + m_ui->irz_sbox->setDisabled(true); + m_ui->itx_sbox->setDisabled(true); + m_ui->ity_sbox->setDisabled(true); + m_ui->itz_sbox->setDisabled(true); + m_ui->angle_uncertain_sbox->setDisabled(true); + m_ui->distance_uncertain_sbox->setDisabled(true); for(size_t i = 0; i < m_ui->sensors_selection_list->count(); i++) { @@ -345,6 +312,25 @@ void CMainWindow::syncObservationsClicked() m_ui->grouped_observations_treeview->setModel(m_sync_model); m_ui->algo_cbox->setDisabled(false); + m_ui->sensor_cbox->setDisabled(false); + m_ui->irx_sbox->setDisabled(false); + m_ui->iry_sbox->setDisabled(false); + m_ui->irz_sbox->setDisabled(false); + m_ui->itx_sbox->setDisabled(false); + m_ui->ity_sbox->setDisabled(false); + m_ui->itz_sbox->setDisabled(false); + m_ui->angle_uncertain_sbox->setDisabled(false); + m_ui->distance_uncertain_sbox->setDisabled(false); + + for(size_t i = 0; i < selected_sensor_labels.size(); i++) + { + QListWidgetItem *item = new QListWidgetItem; + item->setText(QString::fromStdString(selected_sensor_labels[i])); + item->setFlags(item->flags() | Qt::ItemIsUserCheckable); + item->setCheckState(Qt::Checked); + m_ui->sensor_cbox->insertItem(i, QString::fromStdString(selected_sensor_labels[i])); + } + std::string stats_string; stats_string = "GROUPING STATS"; stats_string += "\n- - - - - - - - - - - - - - - - - - - - - - - - - - - - - "; @@ -362,7 +348,7 @@ void CMainWindow::syncObservationsClicked() m_ui->viewer_container->updateText(stats_string); m_ui->viewer_container->updateSensorsList(selected_sensor_labels); - m_ui->viewer_container->updateSensorPoses(m_model->getSensorPoses()); + m_ui->viewer_container->updateSensorPoses(m_sync_model->getSensorPoses()); m_ui->viewer_container->observationsSynced = true; } @@ -371,6 +357,77 @@ void CMainWindow::syncObservationsClicked() } } +void CMainWindow::sensorIndexChanged(int index) +{ + Eigen::Matrix4f rt = m_sync_model->getSensorPoses()[index]; + Eigen::Vector2f uncertain = m_sync_model->getSensorUncertainties()[index]; + + Eigen::Matrix rvec = utils::getRotations(rt); + Eigen::Matrix tvec = utils::getTranslations(rt); + + m_ui->irx_sbox->blockSignals(true); + m_ui->iry_sbox->blockSignals(true); + m_ui->irz_sbox->blockSignals(true); + m_ui->itx_sbox->blockSignals(true); + m_ui->ity_sbox->blockSignals(true); + m_ui->itz_sbox->blockSignals(true); + m_ui->angle_uncertain_sbox->blockSignals(true); + m_ui->distance_uncertain_sbox->blockSignals(true); + + m_ui->irx_sbox->setValue(rvec(0,0)); + m_ui->iry_sbox->setValue(rvec(1,0)); + m_ui->irz_sbox->setValue(rvec(2,0)); + m_ui->itx_sbox->setValue(tvec(0,0)); + m_ui->ity_sbox->setValue(tvec(1,0)); + m_ui->itz_sbox->setValue(tvec(2,0)); + m_ui->angle_uncertain_sbox->setValue(uncertain(0)); + m_ui->distance_uncertain_sbox->setValue(uncertain(1)); + + m_ui->irx_sbox->blockSignals(false); + m_ui->iry_sbox->blockSignals(false); + m_ui->irz_sbox->blockSignals(false); + m_ui->itx_sbox->blockSignals(false); + m_ui->ity_sbox->blockSignals(false); + m_ui->itz_sbox->blockSignals(false); + m_ui->angle_uncertain_sbox->blockSignals(false); + m_ui->distance_uncertain_sbox->blockSignals(false); +} + +void CMainWindow::initCalibChanged(double value) +{ + QDoubleSpinBox *sbox = (QDoubleSpinBox*)sender(); + + int sensor_index = m_ui->sensor_cbox->currentIndex(); + + if(sbox->accessibleName() == QString("angle_uncertain_sbox") || sbox->accessibleName() == QString("distance_uncertain_sbox")) + { + Eigen::Vector2f uncertainty; + uncertainty(0) = m_ui->angle_uncertain_sbox->value(); + uncertainty(1) = m_ui->distance_uncertain_sbox->value(); + m_sync_model->setSensorUncertainty(uncertainty, sensor_index); + } + + else + { + std::vector sensor_poses = m_sync_model->getSensorPoses(); + + Eigen::Matrix rotation_angles; + Eigen::Matrix translations; + + rotation_angles(0) = m_ui->irx_sbox->value() * (M_PI/180); + rotation_angles(1) = m_ui->iry_sbox->value() * (M_PI/180); + rotation_angles(2) = m_ui->irz_sbox->value() * (M_PI/180); + sensor_poses[sensor_index].block(0,0,3,3) = utils::getRotationMatrix(rotation_angles); + + translations(0) = m_ui->itx_sbox->value(); + translations(1) = m_ui->ity_sbox->value(); + translations(2) = m_ui->itz_sbox->value(); + sensor_poses[sensor_index].block(0,3,3,1) = translations; + + m_sync_model->setSensorPose(sensor_poses[sensor_index], sensor_index); + } +} + void CMainWindow::treeItemClicked(const QModelIndex &index) { if(index.isValid()) @@ -414,12 +471,12 @@ void CMainWindow::treeItemClicked(const QModelIndex &index) item->setCloud(cloud); } - if((m_calib_from_planes_gui != nullptr) && (m_calib_from_planes_gui->calibStatus() == CalibrationFromPlanesStatus::PLANES_EXTRACTED - || m_calib_from_planes_gui->calibStatus() == CalibrationFromPlanesStatus::PLANES_MATCHED)) + if((m_calib_from_planes_gui != nullptr) && (m_calib_from_planes_gui->calibStatus() == CalibFromPlanesStatus::PLANES_EXTRACTED + || m_calib_from_planes_gui->calibStatus() == CalibFromPlanesStatus::PLANES_MATCHED)) m_calib_from_planes_gui->publishPlanes(sensor_id, sync_obs_id); - else if((m_calib_from_lines_gui != nullptr) && (m_calib_from_lines_gui->calibStatus() == CalibrationFromLinesStatus::LINES_EXTRACTED - || m_calib_from_lines_gui->calibStatus() == CalibrationFromLinesStatus::LINES_MATCHED)) + else if((m_calib_from_lines_gui != nullptr) && (m_calib_from_lines_gui->calibStatus() == CalibFromLinesStatus::LINES_EXTRACTED + || m_calib_from_lines_gui->calibStatus() == CalibFromLinesStatus::LINES_MATCHED)) m_calib_from_lines_gui->publishLines(sensor_id, sync_obs_id); } @@ -466,20 +523,20 @@ void CMainWindow::treeItemClicked(const QModelIndex &index) item->child(i)->setCloud(cloud); } - if((m_calib_from_planes_gui != nullptr) && (m_calib_from_planes_gui->calibStatus() == CalibrationFromPlanesStatus::PLANES_EXTRACTED - || m_calib_from_planes_gui->calibStatus() == CalibrationFromPlanesStatus::PLANES_MATCHED)) + if((m_calib_from_planes_gui != nullptr) && (m_calib_from_planes_gui->calibStatus() == CalibFromPlanesStatus::PLANES_EXTRACTED + || m_calib_from_planes_gui->calibStatus() == CalibFromPlanesStatus::PLANES_MATCHED)) m_calib_from_planes_gui->publishPlanes(sensor_id, sync_obs_id); - else if((m_calib_from_lines_gui != nullptr) && (m_calib_from_lines_gui->calibStatus() == CalibrationFromLinesStatus::LINES_EXTRACTED - || m_calib_from_lines_gui->calibStatus() == CalibrationFromLinesStatus::LINES_MATCHED)) + else if((m_calib_from_lines_gui != nullptr) && (m_calib_from_lines_gui->calibStatus() == CalibFromLinesStatus::LINES_EXTRACTED + || m_calib_from_lines_gui->calibStatus() == CalibFromLinesStatus::LINES_MATCHED)) m_calib_from_lines_gui->publishLines(sensor_id, sync_obs_id); } - if((m_calib_from_planes_gui != nullptr) && (m_calib_from_planes_gui->calibStatus() == CalibrationFromPlanesStatus::PLANES_MATCHED)) + if((m_calib_from_planes_gui != nullptr) && (m_calib_from_planes_gui->calibStatus() == CalibFromPlanesStatus::PLANES_MATCHED)) m_calib_from_planes_gui->publishCorrespPlanes(item->row()); - else if((m_calib_from_lines_gui != nullptr) && (m_calib_from_lines_gui->calibStatus() == CalibrationFromLinesStatus::LINES_MATCHED)) + else if((m_calib_from_lines_gui != nullptr) && (m_calib_from_lines_gui->calibStatus() == CalibFromLinesStatus::LINES_MATCHED)) m_calib_from_lines_gui->publishCorrespLines(item->row()); } @@ -545,29 +602,11 @@ void CMainWindow::algosIndexChanged(int index) } } -void CMainWindow::initCalibChanged(double value) -{ - QSpinBox *sbox = (QSpinBox*)sender(); - - if(sbox->accessibleName() == QString("irx")) - m_init_calib[0] = value; - else if(sbox->accessibleName() == QString("iry")) - m_init_calib[1] = value; - else if(sbox->accessibleName() == QString("irz")) - m_init_calib[2] = value; - else if(sbox->accessibleName() == QString("itx")) - m_init_calib[3] = value; - else if(sbox->accessibleName() == QString("ity")) - m_init_calib[4] = value; - else if(sbox->accessibleName() == QString("itz")) - m_init_calib[5] = value; -} - void CMainWindow::runCalibFromPlanes(TCalibFromPlanesParams *params) { switch(params->calib_status) { - case CalibrationFromPlanesStatus::PCALIB_YET_TO_START: + case CalibFromPlanesStatus::PCALIB_YET_TO_START: { if(m_sync_model != nullptr && (m_sync_model->getRootItem()->childCount() > 0)) { @@ -587,13 +626,13 @@ void CMainWindow::runCalibFromPlanes(TCalibFromPlanesParams *params) break; } - case CalibrationFromPlanesStatus::PLANES_EXTRACTED: + case CalibFromPlanesStatus::PLANES_EXTRACTED: { m_calib_from_planes_gui->matchPlanes(); break; } - case CalibrationFromPlanesStatus::PLANES_MATCHED: + case CalibFromPlanesStatus::PLANES_MATCHED: { m_calib_from_planes_gui->calibrate(); break; @@ -605,7 +644,7 @@ void CMainWindow::runCalibFromLines(TCalibFromLinesParams *params) { switch(params->calib_status) { - case CalibrationFromLinesStatus::LCALIB_YET_TO_START: + case CalibFromLinesStatus::LCALIB_YET_TO_START: { if(m_sync_model != nullptr && (m_sync_model->getRootItem()->childCount() > 0)) { @@ -623,7 +662,7 @@ void CMainWindow::runCalibFromLines(TCalibFromLinesParams *params) break; } - case CalibrationFromLinesStatus::LINES_EXTRACTED: + case CalibFromLinesStatus::LINES_EXTRACTED: { m_calib_from_lines_gui->matchLines(); break; diff --git a/gui/CMainWindow.h b/gui/CMainWindow.h index 36f29b2..d348467 100644 --- a/gui/CMainWindow.h +++ b/gui/CMainWindow.h @@ -80,9 +80,6 @@ private slots: /** Path of the most recently loaded config file. */ QString m_recent_config_path; - /** The initial calibration vector. */ - std::array m_init_calib; - /** Stores the originally loaded rawlog. */ CObservationTreeGui *m_model; diff --git a/gui/CMainWindow.ui b/gui/CMainWindow.ui index 4748192..0e0cde6 100644 --- a/gui/CMainWindow.ui +++ b/gui/CMainWindow.ui @@ -17,7 +17,7 @@ mainwindow - + @@ -34,7 +34,7 @@ - + 0 @@ -44,143 +44,170 @@ 382 - 250 + 411 - - - 382 - 250 - + + config_dock QDockWidget::DockWidgetFloatable|QDockWidget::DockWidgetMovable - Setup + Algorithm Configuration - 1 + 2 - + - + 0 0 - - - 0 - - - 9 - + + + 0 + 0 + + + + + 382 + 16777215 + + + + config_content + + - - - + + + Qt::Horizontal + + + 8 + + + - + + + + + Select Sensors: + + + + + + + false + + + + 0 + 1 + + + + + 16777215 + 80 + + + + + + + + + + + + Max delay (ms): + + + + + + + false + + + 0 + + + 5000 + + + 10 + + + 30 + + + + + + + false - - - 1 - 0 - - - - false - - - Qt::LogicalMoveStyle + + Sync - - - false + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + - ... + Grouped observations: - - - - - - - 1 - 0 - - - - Select Rawlog File: - - - - - - - Load Config File: - - - - - - + + + false + - - 1 + + 0 0 - - false - - + false - - - Qt::LogicalMoveStyle - - - - - - - ... - + - - - - - - - false - - - Load Rawlog - - - false - - - false - + - + Qt::Horizontal @@ -206,37 +233,53 @@ - - - - iTx: - - - - - + + false - irx + itz + + + 5 - -359.990000000000009 + -10.000000000000000 - 359.990000000000009 + 10.000000000000000 - 0.500000000000000 + 0.050000000000000 + + + + + + + iYaw: + + + + + + + iTx: - - + + false + + itx + + + 5 + -10.000000000000000 @@ -244,14 +287,14 @@ 10.000000000000000 - 0.500000000000000 + 0.050000000000000 - - + + - iTy: + iPitch: @@ -263,6 +306,9 @@ irz + + 5 + -359.990000000000009 @@ -274,20 +320,30 @@ - - + + - MED: + iTz: - - + + + + iTy: + + + + + false - itz + iry + + + 5 -359.990000000000009 @@ -300,32 +356,38 @@ - - + + false - itx + ity + + + 5 - -359.990000000000009 + -10.000000000000000 - 359.990000000000009 + 10.000000000000000 - 0.500000000000000 + 0.050000000000000 - - + + false - iry + irx + + + 5 -359.990000000000009 @@ -338,172 +400,137 @@ - - + + - iRy: + iRoll: - - + + + + + + - iTz: + Angular uncertainty: - - + + false - ity - - - -359.990000000000009 + angle_uncertain_sbox - 359.990000000000009 - - - 0.500000000000000 + 50.000000000000000 - - + + + + + + - iRz: + Distance uncertainty: - - - - iRx: + + + + false + + + distance_uncertain_sbox + + + 50.000000000000000 + + + 0.050000000000000 - + Qt::Horizontal - - - - - - - 0 - 0 - - - - QDockWidget::DockWidgetFloatable|QDockWidget::DockWidgetMovable - - - Observations - - - 1 - - - - - 9 - - - 9 - - - - 1 - - - Qt::Vertical - - - 8 - - - - - 0 - - - 9 + + + + + Calibration Algorithm: - - 9 + + + + + + false - - 9 + + + 0 + 0 + - - - List - - - - - - - false - - + + + - - - - - - 0 - - - 9 - - - 9 - - - 9 - - - - Description - - + + Plane matching + - - - false - - - - 0 - 0 - - - + + Line Matching + - - - + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + - + + + TopToolBarArea + + + false + + + 0 @@ -513,150 +540,232 @@ 382 - 316 + 124 - - config_dock + + + 382 + 124 + QDockWidget::DockWidgetFloatable|QDockWidget::DockWidgetMovable - Algorithm Configuration + Setup - 2 + 1 - + - + 0 0 - - - 0 - 0 - - 382 16777215 - - config_content - - + - - - Qt::Horizontal - - - 8 - - - + + + - - - - - Select Sensors: - - - - - - - false - - - - 0 - 1 - - - - - 16777215 - 80 - - - - - + + + false + + + + 1 + 0 + + + + false + + + Qt::LogicalMoveStyle + + - - - - - Max delay (ms): - - - - - - - false - - - 0 - - - 5000 - - - 10 - - - 30 - - - - + + + false + + + ... + + + + + + + + + 1 + 0 + + + + Select Rawlog File: + + + + + + + Load Config File: + + + + + - - + + + + 1 + 0 + + + false + + false + + + Qt::LogicalMoveStyle + + + + + - Sync + ... + + + + + + + + false + + + Load Rawlog + + + false + + + false + + + + + + + + + + 0 + 0 + + + + QDockWidget::DockWidgetFloatable|QDockWidget::DockWidgetMovable + + + Observations + + + 1 + + + + + 0 + 0 + + + + + + + 1 + + + Qt::Vertical + + + 8 + + + + + 0 + + + 9 + + + 0 + + + 9 + - - - Qt::Vertical + + + List - - - 20 - 40 - + + + + + + false - + - + + + 0 + + + 9 + + + 0 + + + 9 + - + - Grouped observations: + Description - + false @@ -666,85 +775,15 @@ 0 - - false - - - - - Qt::Horizontal - - - - - - - - - Calibration Algorithm: - - - - - - - false - - - - 0 - 0 - - - - - - - - - - Plane matching - - - - - Line Matching - - - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - TopToolBarArea - - - false - - diff --git a/gui/config/CCalibFromLinesConfig.cpp b/gui/config/CCalibFromLinesConfig.cpp index 14e1d7b..c9e0fc1 100644 --- a/gui/config/CCalibFromLinesConfig.cpp +++ b/gui/config/CCalibFromLinesConfig.cpp @@ -31,8 +31,6 @@ void CCalibFromLinesConfig::setConfig(mrpt::config::CConfigFile &config_file) m_ui->chightolow_ratio_sbox->setValue(m_config_file.read_int("line_segmentation", "canny_high_to_low_ratio", 3, true)); m_ui->ckernel_size_sbox->setValue(m_config_file.read_double("line_segmentation", "canny_kernel_size", 3, true)); m_ui->hthreshold_sbox->setValue(m_config_file.read_int("line_segmentation", "hough_threshold", 150, true)); - m_ui->min_normals_dot_prod_sbox->setValue(m_config_file.read_double("line_matching", "min_normals_dot_product", 0.90, true)); - m_ui->max_line_normal_dot_prod_sbox->setValue(m_config_file.read_double("line_matching", "max_line_normal_dot_product", 0.10, true)); } void CCalibFromLinesConfig::extractLinesClicked() @@ -42,16 +40,14 @@ void CCalibFromLinesConfig::extractLinesClicked() m_params.seg.chigh_to_low_ratio = m_ui->chightolow_ratio_sbox->value(); m_params.seg.ckernel_size = m_ui->ckernel_size_sbox->value(); m_params.seg.hthreshold = m_ui->hthreshold_sbox->value(); - m_params.calib_status = CalibrationFromLinesStatus::LCALIB_YET_TO_START; + m_params.calib_status = CalibFromLinesStatus::LCALIB_YET_TO_START; m_ui->match_lines_button->setDisabled(false); static_cast(parentWidget()->parentWidget()->parentWidget())->runCalibFromLines(&m_params); } void CCalibFromLinesConfig::matchLinesClicked() { - m_params.match.min_normals_dot_prod = m_ui->min_normals_dot_prod_sbox->value(); - m_params.match.max_line_normal_dot_prod = m_ui->max_line_normal_dot_prod_sbox->value(); - m_params.calib_status = CalibrationFromLinesStatus::LINES_EXTRACTED; + m_params.calib_status = CalibFromLinesStatus::LINES_EXTRACTED; m_ui->calib_button->setDisabled(false); static_cast(parentWidget()->parentWidget()->parentWidget())->runCalibFromLines(&m_params); } diff --git a/gui/config/CCalibFromLinesConfig.ui b/gui/config/CCalibFromLinesConfig.ui index 3a6fbb5..c797e9a 100644 --- a/gui/config/CCalibFromLinesConfig.ui +++ b/gui/config/CCalibFromLinesConfig.ui @@ -7,15 +7,38 @@ 0 0 278 - 365 + 279 Form + + 0 + + + 0 + + + + + Downsampling factor: + + + + + + + 1 + + + 10 + + + @@ -125,80 +148,6 @@ - - - - <html><head/><body><p><span style=" font-style:italic;">Line Matching</span></p></body></html> - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - min n1.n2 - - - - - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.900000000000000 - - - - - - - max v1.n2 - - - - - - - 1.000000000000000 - - - 0.100000000000000 - - - - - - - Downsampling factor - - - - - - - 1 - - - 10 - - - diff --git a/gui/config/CCalibFromPlanesConfig.cpp b/gui/config/CCalibFromPlanesConfig.cpp index 276f165..cda6cc4 100644 --- a/gui/config/CCalibFromPlanesConfig.cpp +++ b/gui/config/CCalibFromPlanesConfig.cpp @@ -51,8 +51,6 @@ void CCalibFromPlanesConfig::setConfig(mrpt::config::CConfigFile &config_file) m_ui->max_cos_normal_sbox->setValue(m_config_file.read_double("plane_segmentation", "max_cos_normal", 0.998, true)); m_ui->dist_centre_plane_sbox->setValue(m_config_file.read_double("plane_segmentation", "dist_centre_plane_threshold", 0.1, true)); m_ui->proximity_threshold_sbox->setValue(m_config_file.read_double("plane_segmentation", "proximity_threshold", 0.4, true)); - m_ui->min_normals_dot_sbox->setValue(m_config_file.read_double("plane_matching", "min_normals_dot_product", 0.9, true)); - m_ui->max_dist_diff_sbox->setValue(m_config_file.read_double("plane_matching", "max_plane_dist_diff", 0.2, true)); m_ui->max_iters_sbox->setValue(m_config_file.read_int("solver", "max_iters", 10, true)); m_ui->min_update_sbox->setValue(m_config_file.read_double("solver", "min_update", 0.00001, true)); m_ui->converge_error_sbox->setValue(m_config_file.read_double("solver", "convergence_error", 0.00001, true)); @@ -72,16 +70,14 @@ void CCalibFromPlanesConfig::extractPlanes() m_params.seg.max_cos_normal = m_ui->max_cos_normal_sbox->value(); m_params.seg.dist_centre_plane_threshold = m_ui->dist_centre_plane_sbox->value(); m_params.seg.proximity_threshold = m_ui->proximity_threshold_sbox->value(); - m_params.calib_status = CalibrationFromPlanesStatus::PCALIB_YET_TO_START; + m_params.calib_status = CalibFromPlanesStatus::PCALIB_YET_TO_START; static_cast(parentWidget()->parentWidget()->parentWidget())->runCalibFromPlanes(&m_params); m_ui->match_planes_button->setDisabled(false); } void CCalibFromPlanesConfig::matchPlanes() { - m_params.match.min_normals_dot_prod = m_ui->min_normals_dot_sbox->value(); - m_params.match.max_dist_diff = m_ui->max_dist_diff_sbox->value(); - m_params.calib_status = CalibrationFromPlanesStatus::PLANES_EXTRACTED; + m_params.calib_status = CalibFromPlanesStatus::PLANES_EXTRACTED; m_ui->calib_button->setDisabled(false); static_cast(parentWidget()->parentWidget()->parentWidget())->runCalibFromPlanes(&m_params); } @@ -91,7 +87,7 @@ void CCalibFromPlanesConfig::calibrate() m_params.solver.max_iters = m_ui->max_iters_sbox->value(); m_params.solver.min_update = m_ui->min_update_sbox->value(); m_params.solver.converge_error = m_ui->converge_error_sbox->value(); - m_params.calib_status = CalibrationFromPlanesStatus::PLANES_MATCHED; + m_params.calib_status = CalibFromPlanesStatus::PLANES_MATCHED; static_cast(parentWidget()->parentWidget()->parentWidget())->runCalibFromPlanes(&m_params); } diff --git a/gui/config/CCalibFromPlanesConfig.ui b/gui/config/CCalibFromPlanesConfig.ui index 0c4fe89..c4d98d9 100644 --- a/gui/config/CCalibFromPlanesConfig.ui +++ b/gui/config/CCalibFromPlanesConfig.ui @@ -7,7 +7,7 @@ 0 0 382 - 672 + 705 @@ -26,8 +26,54 @@ Form + + 0 + + + 0 + + + + + Downsampling factor: + + + + + + + 1 + + + 10 + + + + + + + <html><head/><body><p><span style=" font-style:italic;">Plane Segmentation</span></p></body></html> + + + Qt::RichText + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + @@ -200,96 +246,70 @@ - - + + - min n1.n2: + Max Planes Cos Normal: - - + + + + 3 + 1.000000000000000 - 0.050000000000000 + 0.100000000000000 - 0.900000000000000 + 0.998000000000000 - - + + - max d1 - d2: + Planes Centre Threshold: - - - - 1.000000000000000 - + + - 0.050000000000000 + 0.100000000000000 - 0.200000000000000 + 0.100000000000000 - - + + - <html><head/><body><p><span style=" font-style:italic;">Plane Matching</span></p></body></html> - - - Qt::RichText + Proximity Threshold: - - - - Qt::Horizontal - - - - 40 - 20 - + + + + 2 - - - - - - <html><head/><body><p><span style=" font-style:italic;">Plane Segmentation</span></p></body></html> + + 0.100000000000000 - - Qt::RichText + + 0.400000000000000 - - - - Qt::Horizontal - - - - 40 - 20 - - - - - + - + <html><head/><body><p><span style=" font-style:italic;">Solver</span></p></body></html> @@ -300,7 +320,7 @@ - + Qt::Horizontal @@ -313,14 +333,14 @@ - + Maximum Iterations: - + 1000 @@ -330,14 +350,14 @@ - + Minimum update: - + 5 @@ -350,14 +370,14 @@ - + Convergence Error: - + 5 @@ -370,83 +390,6 @@ - - - - Downsampling factor: - - - - - - - 1 - - - 10 - - - - - - - Planes Centre Threshold: - - - - - - - Proximity Threshold: - - - - - - - 3 - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.998000000000000 - - - - - - - 0.100000000000000 - - - 0.100000000000000 - - - - - - - 2 - - - 0.100000000000000 - - - 0.400000000000000 - - - - - - - Max Planes Cos Normal: - - - diff --git a/gui/core_gui/CCalibFromLinesGui.cpp b/gui/core_gui/CCalibFromLinesGui.cpp index 977bad2..91b8fe2 100644 --- a/gui/core_gui/CCalibFromLinesGui.cpp +++ b/gui/core_gui/CCalibFromLinesGui.cpp @@ -5,14 +5,11 @@ using namespace mrpt::obs; CCalibFromLinesGui::CCalibFromLinesGui(CObservationTree *model, TCalibFromLinesParams *params) : - CCalibFromLines(model) -{ - m_params = params; -} + CCalibFromLines(model, params) +{} CCalibFromLinesGui::~CCalibFromLinesGui() -{ -} +{} void CCalibFromLinesGui::addTextObserver(CTextObserver *observer) { @@ -80,9 +77,9 @@ void CCalibFromLinesGui::publishCorrespLines(const int &obs_set_id) } } -CalibrationFromLinesStatus CCalibFromLinesGui::calibStatus() +CalibFromLinesStatus CCalibFromLinesGui::calibStatus() { - return m_params->calib_status; + return params->calib_status; } void CCalibFromLinesGui::extractLines() @@ -111,7 +108,7 @@ void CCalibFromLinesGui::extractLines() mvv_lines[i].resize((sync_model->getSyncIndices()[i]).size()); //let's run it for 15 sets - for(size_t j = 0; j < 15; j += m_params->downsample_factor) + for(size_t j = 0; j < 15; j += params->downsample_factor) { tree_item = root_item->child(j); @@ -126,7 +123,7 @@ void CCalibFromLinesGui::extractLines() line_segment_start = pcl::getTime(); segmented_lines.clear(); - segmentLines(image, range, m_params->seg, obs_item->cameraParamsIntensity, segmented_lines); + segmentLines(image, range, obs_item->cameraParamsIntensity, segmented_lines); line_segment_end = pcl::getTime(); n_lines = segmented_lines.size(); @@ -152,7 +149,7 @@ void CCalibFromLinesGui::extractLines() publishText(s); - m_params->calib_status = CalibrationFromLinesStatus::LINES_EXTRACTED; + params->calib_status = CalibFromLinesStatus::LINES_EXTRACTED; } void CCalibFromLinesGui::matchLines() @@ -171,7 +168,7 @@ void CCalibFromLinesGui::matchLines() std::vector used_sets; //for(int i = 0; i < root_item->childCount(); i++) - for(int i = 0; i < 15; i += m_params->downsample_factor) + for(int i = 0; i < 15; i += params->downsample_factor) { tree_item = root_item->child(i); lines.resize(sensor_labels.size()); @@ -186,7 +183,7 @@ void CCalibFromLinesGui::matchLines() lines[sensor_id] = mvv_lines[sensor_id][sync_obs_id]; } - findPotentialMatches(lines, i, m_params->match); + findPotentialMatches(lines, i); lines.clear(); //print statistics @@ -216,5 +213,5 @@ void CCalibFromLinesGui::matchLines() publishText(s); - m_params->calib_status = CalibrationFromLinesStatus::LINES_MATCHED; + params->calib_status = CalibFromLinesStatus::LINES_MATCHED; } diff --git a/gui/core_gui/CCalibFromLinesGui.h b/gui/core_gui/CCalibFromLinesGui.h index 66a99cc..ca531c3 100644 --- a/gui/core_gui/CCalibFromLinesGui.h +++ b/gui/core_gui/CCalibFromLinesGui.h @@ -58,14 +58,11 @@ class CCalibFromLinesGui : public CCalibFromLines void publishCorrespLines(const int &obs_set_id); /** Returns the status of the calibration progress. */ - CalibrationFromLinesStatus calibStatus(); + CalibFromLinesStatus calibStatus(); private: - /** The parameters for the calibration. */ - TCalibFromLinesParams *m_params; - /** List of text observers to be notified about the progress. */ std::vector m_text_observers; diff --git a/gui/core_gui/CCalibFromPlanesGui.cpp b/gui/core_gui/CCalibFromPlanesGui.cpp index e762169..495d6fb 100755 --- a/gui/core_gui/CCalibFromPlanesGui.cpp +++ b/gui/core_gui/CCalibFromPlanesGui.cpp @@ -19,14 +19,11 @@ using namespace mrpt::obs; CCalibFromPlanesGui::CCalibFromPlanesGui(CObservationTreeGui *model, TCalibFromPlanesParams *params) : - CCalibFromPlanes(model) -{ - m_params = params; -} + CCalibFromPlanes(model, params) +{} CCalibFromPlanesGui::~CCalibFromPlanesGui() -{ -} +{} void CCalibFromPlanesGui::addTextObserver(CTextObserver *observer) { @@ -94,9 +91,9 @@ void CCalibFromPlanesGui::publishCorrespPlanes(const int &obs_set_id) } } -CalibrationFromPlanesStatus CCalibFromPlanesGui::calibStatus() +CalibFromPlanesStatus CCalibFromPlanesGui::calibStatus() { - return m_params->calib_status; + return params->calib_status; } void CCalibFromPlanesGui::run() @@ -135,7 +132,7 @@ void CCalibFromPlanesGui::extractPlanes() publishText("**Extracting planes from " + selected_sensor_labels[i] + " observations**"); mvv_planes[i].resize((sync_model->getSyncIndices()[i]).size()); - for(size_t j = 0; j < 15; j += m_params->downsample_factor) + for(size_t j = 0; j < 15; j += params->downsample_factor) { tree_item = root_item->child(j); @@ -157,7 +154,7 @@ void CCalibFromPlanesGui::extractPlanes() plane_segment_start = pcl::getTime(); segmented_planes.clear(); - segmentPlanes(cloud, m_params->seg, segmented_planes); + segmentPlanes(cloud, segmented_planes); plane_segment_end = pcl::getTime(); n_planes = segmented_planes.size(); @@ -183,7 +180,7 @@ void CCalibFromPlanesGui::extractPlanes() publishText(s); - m_params->calib_status = CalibrationFromPlanesStatus::PLANES_EXTRACTED; + params->calib_status = CalibFromPlanesStatus::PLANES_EXTRACTED; } void CCalibFromPlanesGui::matchPlanes() @@ -202,7 +199,7 @@ void CCalibFromPlanesGui::matchPlanes() std::vector used_sets; //for(int i = 0; i < root_item->childCount(); i++) - for(int i = 0; i < 15; i+= m_params->downsample_factor) + for(int i = 0; i < 15; i+= params->downsample_factor) { tree_item = root_item->child(i); planes.resize(sensor_labels.size()); @@ -217,7 +214,7 @@ void CCalibFromPlanesGui::matchPlanes() planes[sensor_id] = mvv_planes[sensor_id][sync_obs_id]; } - findPotentialMatches(planes, i, m_params->match); + findPotentialMatches(planes, i); planes.clear(); //print statistics @@ -246,15 +243,28 @@ void CCalibFromPlanesGui::matchPlanes() publishText(s); - m_params->calib_status = CalibrationFromPlanesStatus::PLANES_MATCHED; + params->calib_status = CalibFromPlanesStatus::PLANES_MATCHED; } void CCalibFromPlanesGui::calibrate() { publishText("****Running the calibration solver****"); + computeRotation(); + + publishText("**Results of the rotation solver**"); std::string stats; - computeRotation(m_params->solver, sync_model->getSensorPoses(), stats); + std::stringstream stream; + + for(int sensor_id = 0; sensor_id < sync_model->getNumberOfSensors(); sensor_id++) + stream << result.estimate[sensor_id].block(0,0,3,3) << "\n"; + + stats = "Status: " + result.msg; + stats += "\nInitial error: " + std::to_string(result.init_error); + stats += "\nNumber of iterations: " + std::to_string(result.num_iters); + stats += "\nFinal error: " + std::to_string(result.final_error); + stats += "\n\nEstimated rotation: \n"; + stats += stream.str(); publishText(stats); } diff --git a/gui/core_gui/CCalibFromPlanesGui.h b/gui/core_gui/CCalibFromPlanesGui.h index fb1be6b..8737f2d 100755 --- a/gui/core_gui/CCalibFromPlanesGui.h +++ b/gui/core_gui/CCalibFromPlanesGui.h @@ -64,13 +64,10 @@ class CCalibFromPlanesGui : public CCalibFromPlanes void publishCorrespPlanes(const int &obs_set_id); /** Returns the status of the calibration progress. */ - CalibrationFromPlanesStatus calibStatus(); + CalibFromPlanesStatus calibStatus(); private: - /** The parameters for the calibration. */ - TCalibFromPlanesParams *m_params; - /** List of text observers to be notified about the progress. */ std::vector m_text_observers;