diff --git a/rct_examples/CMakeLists.txt b/rct_examples/CMakeLists.txt index 6e495b8a..a5ac643f 100644 --- a/rct_examples/CMakeLists.txt +++ b/rct_examples/CMakeLists.txt @@ -125,13 +125,13 @@ target_link_libraries(${PROJECT_NAME}_camera_intrinsic_calibration_validation ${ #Executable for testing camera noise -add_executable(${PROJECT_NAME}_noise_test_2d src/validation/noise_qualify_2d.cpp) +add_executable(${PROJECT_NAME}_noise_qualification_2d src/tools/noise_qualification_2d.cpp) -set_target_properties(${PROJECT_NAME}_noise_test_2d PROPERTIES OUTPUT_NAME noise_test_2d PREFIX "") +set_target_properties(${PROJECT_NAME}_noise_qualification_2d PROPERTIES OUTPUT_NAME noise_qualification_2d PREFIX "") -add_dependencies(${PROJECT_NAME}_noise_test_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${PROJECT_NAME}_noise_qualification_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_noise_test_2d ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools) +target_link_libraries(${PROJECT_NAME}_noise_qualification_2d ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools) ############# ## Testing ## @@ -154,6 +154,7 @@ install(TARGETS ${PROJECT_NAME}_intr ${PROJECT_NAME}_pnp ${PROJECT_NAME}_camera_intrinsic_calibration_validation + ${PROJECT_NAME}_noise_qualification_2d ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/rct_examples/config/noise_qualification/camera_to_target_guess.yaml b/rct_examples/config/noise_qualification/camera_to_target_guess.yaml new file mode 100644 index 00000000..645780cf --- /dev/null +++ b/rct_examples/config/noise_qualification/camera_to_target_guess.yaml @@ -0,0 +1,8 @@ +camera_to_target_guess: + x: 0.00330134 + y: 0.0778156 + z: 0.77218 + qx: 0.999656 + qy: 0.00396669 + qz: 0.020924 + qw: 0.0152998 diff --git a/rct_examples/config/noise_qualification/data.yaml b/rct_examples/config/noise_qualification/data.yaml new file mode 100644 index 00000000..58160ed0 --- /dev/null +++ b/rct_examples/config/noise_qualification/data.yaml @@ -0,0 +1,16 @@ +# List of images relative to a data directory +# In this example case all the images are the same to produce a zero-noise result +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png +- image: images/0.png diff --git a/rct_examples/launch/noise_qualification_2d.launch b/rct_examples/launch/noise_qualification_2d.launch new file mode 100644 index 00000000..eb7d2907 --- /dev/null +++ b/rct_examples/launch/noise_qualification_2d.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/rct_examples/src/tools/noise_qualification_2d.cpp b/rct_examples/src/tools/noise_qualification_2d.cpp new file mode 100644 index 00000000..f13004ae --- /dev/null +++ b/rct_examples/src/tools/noise_qualification_2d.cpp @@ -0,0 +1,109 @@ +#include +#include +#include +#include "rct_ros_tools/data_set.h" +#include "rct_ros_tools/parameter_loaders.h" +#include +#include + +template +bool get(const ros::NodeHandle &nh, const std::string &key, T &val) +{ + if (!nh.getParam(key, val)) + { + ROS_ERROR_STREAM("Failed to get '" << key << "' parameter"); + return false; + } + return true; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "noise_qualification_2d"); + ros::NodeHandle pnh("~"); + + // Parse parameters + std::string data_path; + if (!get(pnh, "data_path", data_path)) + return -1; + + std::string data_file; + if (!get(pnh, "data_file", data_file)) + return -1; + + try + { + // Load the target definition and observation finder + rct_image_tools::ModifiedCircleGridTarget target = rct_ros_tools::loadTarget(pnh, "target_definition"); + rct_image_tools::ModifiedCircleGridObservationFinder obs_finder(target); + + // Load camera intrinsics + rct_optimizations::CameraIntrinsics camera = rct_ros_tools::loadIntrinsics(pnh, "intrinsics"); + + // Load an initial guess for the camera to target transformation + Eigen::Isometry3d camera_to_target_guess = rct_ros_tools::loadPose(pnh, "camera_to_target_guess"); + + // Load the data file which specifies the location of the images on which to perform the noise qualification + YAML::Node root = YAML::LoadFile(data_file); + + // Set up the noise qualification inputs + std::vector problem_set; + problem_set.reserve(root.size()); + for (std::size_t i = 0; i < root.size(); ++i) + { + // Each entry should have an image path. This path is relative to the root_path directory! + const auto img_path = root[i]["image"].as(); + const std::string image_name = data_path + "/" + img_path; + static cv::Mat image = rct_ros_tools::readImageOpenCV(image_name); + + // Find the observations in the image + auto maybe_obs = obs_finder.findObservations(image); + if (!maybe_obs) + { + cv::imshow("points", image); + ROS_INFO_STREAM("Hit enter in the OpenCV window to continue"); + cv::waitKey(); + continue; + } + else + { + // Show the points we detected + cv::imshow("points", obs_finder.drawObservations(image, *maybe_obs)); + ROS_INFO_STREAM("Hit enter in the OpenCV window to continue"); + cv::waitKey(); + } + + // Set up the PnP problem for this image + rct_optimizations::PnPProblem problem; + problem.intr = camera; + problem.camera_to_target_guess = camera_to_target_guess; + + // Add the detected correspondences + problem.correspondences.reserve(maybe_obs->size()); + for (std::size_t j = 0; j < maybe_obs->size(); ++j) + { + problem.correspondences.emplace_back(maybe_obs->at(j), target.points.at(j)); + }; + + problem_set.push_back(problem); + } + + // Perform the noise qualification + rct_optimizations::PnPNoiseStat result = rct_optimizations::qualifyNoise2D(problem_set); + + // Print the results + Eigen::IOFormat fmt(4, 0, ",", "\n", "[", "]"); + ROS_INFO_STREAM("Camera to Target Noise Results"); + ROS_INFO_STREAM("Position mean (m)\n" << result.p_stat.mean.transpose().format(fmt)); + ROS_INFO_STREAM("Position standard deviation (m)\n" << result.p_stat.stdev.transpose().format(fmt)); + ROS_INFO_STREAM("Quaternion mean (qx, qy, qz, qw)\n" << result.q_stat.mean.coeffs().transpose().format(fmt)); + ROS_INFO_STREAM("Quaternion standard deviation\n" << result.q_stat.stdev); + } + catch (const std::exception &ex) + { + ROS_ERROR_STREAM(ex.what()); + return -1; + } + + return 0; +} diff --git a/rct_examples/src/validation/noise_qualify_2d.cpp b/rct_examples/src/validation/noise_qualify_2d.cpp deleted file mode 100644 index 556e3442..00000000 --- a/rct_examples/src/validation/noise_qualify_2d.cpp +++ /dev/null @@ -1,96 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include "rct_ros_tools/parameter_loaders.h" -#include "rct_ros_tools/data_set.h" - -int main(int argc, char** argv) -{ - ros::NodeHandle pnh("~"); - - // Parse parameters - std::string path; - if (!pnh.getParam("data_path", path)) - { - ROS_ERROR("Must set 'data_path' parameter"); - return 1; - } - - int rows; - if (!pnh.getParam("rows", rows)) - { - ROS_ERROR("Must set 'rows' parameter"); - return 1; - } - - int cols; - if (!pnh.getParam("columns", cols)) - { - ROS_ERROR("Must set 'columns' parameter"); - return 1; - } - - //make target - rct_image_tools::ModifiedCircleGridTarget target (rows, cols, 0.01); - rct_image_tools::ModifiedCircleGridObservationFinder obs_finder(target); - - YAML::Node root = YAML::LoadFile(path + std::string("/data.yaml")); - - //Load camera intrinsics - rct_optimizations::CameraIntrinsics camera = rct_ros_tools::loadIntrinsics(path + std::string("/camera_data.yaml")); - - //reserve observations - std::vector problem_set; - problem_set.reserve(root.size()); - - //create observations - for (std::size_t i = 0; i < root.size(); ++i) - { - rct_optimizations::PnPProblem problem; - problem.intr = camera; - - const auto pose_path = root[i]["pose"].as(); - problem.camera_to_target_guess = rct_ros_tools::loadPose(path + pose_path); - - // Each entry should have a pose and image path. This path is relative to the root_path directory! - const auto img_path = root[i]["image"].as(); - const std::string image_name = path + img_path; - static cv::Mat image = rct_ros_tools::readImageOpenCV(image_name); - - auto maybe_obs = obs_finder.findObservations(image); - if (!maybe_obs) - { - cv::imshow("points", image); - cv::waitKey(); - continue; - } - - // And loop over each detected dot: - for (std::size_t j = 0; j < maybe_obs->size(); ++j) - { - problem.correspondences[j].in_image = maybe_obs->at(j); // The obs finder and target define their points in the same order! - problem.correspondences[j].in_target = target.points[j]; - }; - } - - //start with a good guess - - rct_optimizations::PnPNoiseStat output = rct_optimizations::qualifyNoise2D(problem_set); - - - ROS_INFO_STREAM("The standard deviation is : x: " << output.x.std_dev << - "\n y: " << output.y.std_dev << - "\n z: " << output.z.std_dev << - "\n i: " << output.q.qx.std_dev << - "\n j: " << output.q.qy.std_dev << - "\n k: " << output.q.qz.std_dev << - "\n w: " << output.q.qw.std_dev << - "\n"); -} diff --git a/rct_optimizations/CMakeLists.txt b/rct_optimizations/CMakeLists.txt index ad33301a..291d2756 100644 --- a/rct_optimizations/CMakeLists.txt +++ b/rct_optimizations/CMakeLists.txt @@ -25,7 +25,7 @@ add_library(${PROJECT_NAME} SHARED # Validation Tools src/${PROJECT_NAME}/validation/camera_intrinsic_calibration_validation.cpp #Noise Qualification - src/validation/noise_qualifier.cpp + src/${PROJECT_NAME}/validation/noise_qualification.cpp ) target_compile_options(${PROJECT_NAME} PUBLIC -std=c++14) diff --git a/rct_optimizations/include/rct_optimizations/validation/noise_qualification.h b/rct_optimizations/include/rct_optimizations/validation/noise_qualification.h new file mode 100644 index 00000000..93ec1b4d --- /dev/null +++ b/rct_optimizations/include/rct_optimizations/validation/noise_qualification.h @@ -0,0 +1,70 @@ +#pragma once + +#include +#include "rct_optimizations/types.h" +#include "rct_optimizations/pnp.h" + +namespace rct_optimizations +{ +/** + * @brief Holds the mean and standard deviation of a position vector + */ +struct PositionStats +{ + Eigen::Vector3d mean; + Eigen::Vector3d stdev; +}; + +/** +* @brief Contains the mean and standard deviation of a quaternion orientation +*/ +struct QuaternionStats +{ + Eigen::Quaterniond mean; + double stdev; +}; + +/** +* @brief Noise statistics for a position vector and quaternion orientation +*/ +struct PnPNoiseStat +{ + PositionStats p_stat; + QuaternionStats q_stat; +}; + +/** + * @brief Computes the mean and standard deviation of a set of quaternions + * @param quaternions + * @return + */ +QuaternionStats computeQuaternionStats(const std::vector &quaternions); + +/** + * @brief Computes the mean of a set of quaternions + * @param orientations + * @return + */ +Eigen::Quaterniond computeQuaternionMean(const std::vector& orientations); + +/** + * @brief This function qualifies 2D sensor noise by + * comparing PnP results from images taken at same pose. + * Sensor noise can be understood by inspecting the returned standard + * deviations. + * @param Sets of PnP 2D problem parameters + * @return Noise Statistics: a vector of means & std devs + */ +PnPNoiseStat qualifyNoise2D(const std::vector& params); + +/** + * @brief This function qualifies 3D sensor noise by + * comparing PnP results from scans taken at the same pose. + * Sensor noise can be understood by inspecting the returned standard + * deviations. + * @param params 3D image parameters + * @return Noise Statiscics: a vector of standard deviations and the mean pos + */ +PnPNoiseStat qualifyNoise3D(const std::vector& params); + +} // namespace rct_optimizations diff --git a/rct_optimizations/include/rct_optimizations/validation/noise_qualifier.h b/rct_optimizations/include/rct_optimizations/validation/noise_qualifier.h deleted file mode 100644 index 1969882d..00000000 --- a/rct_optimizations/include/rct_optimizations/validation/noise_qualifier.h +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once -#include -#include "rct_optimizations/types.h" -#include "rct_optimizations/pnp.h" - -namespace rct_optimizations -{ - -/** - * @brief The NoiseStatistics struct output structure for image analysis data - */ - struct NoiseStatistics - { - double mean; - double std_dev; - }; - /** - * @brief The RotationStat struct - */ - struct RotationStat - { - NoiseStatistics qx; - NoiseStatistics qy; - NoiseStatistics qz; - NoiseStatistics qw; - }; - /** - * @brief The PnPNoiseStat struct A collection of NoiseStatistics in a form - * relevant to a position & orientation; xyz, quaternion - */ - struct PnPNoiseStat - { - NoiseStatistics x; - NoiseStatistics y; - NoiseStatistics z; - RotationStat q; - }; - - /** - * @brief FindQuaternionMean - * @param orientations - * @return - */ - RotationStat FindQuaternionMean(const std::vector& orientations); - - /** - * @brief qualifyNoise2D This function qualifies 2d sensor noise by - * comparing PnP results from images taken with the same poses. - * Sensor noise can be understood by inspecting the returned standard - * deviations. - * @param Sets of PnP 2D problem parameters - * @return Noise Statistics: a vector of means & std devs - */ -PnPNoiseStat qualifyNoise2D(const std::vector& params); - - /** - * @brief qualifyNoise3D This function qualifies 3d sensor noise by - * comparing PnP results from scans taken with the same poses. - * Sensor noise can be understood by inspecting the returned standard - * deviations. - * @param params 3D image parameters - * @return Noise Statiscics: a vector of standard deviations and the mean pos - */ - PnPNoiseStat qualifyNoise3D(const std::vector& params); - -}//rct_optimizations diff --git a/rct_optimizations/src/rct_optimizations/validation/noise_qualification.cpp b/rct_optimizations/src/rct_optimizations/validation/noise_qualification.cpp new file mode 100644 index 00000000..aed7aac5 --- /dev/null +++ b/rct_optimizations/src/rct_optimizations/validation/noise_qualification.cpp @@ -0,0 +1,165 @@ +#include +#include +#include + +//accumulators +#include +#include +#include +#include +#include + +namespace rct_optimizations +{ + +Eigen::Quaterniond computeQuaternionMean(const std::vector& quaterns) +{ + /* Mean quaternion is found using method described by Markley et al: Quaternion Averaging + * https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf + * + * M = sum(w_i * q_i * q_i^T) Eq. 12 + * q_bar = argmax(q^T * M * q) Eq. 13 + * + * The solution of this maximization problem is well known. The average quaternion is + * the eigenvector of M corresponding to the maximum eigenvalue. + * + * In the above equations, w_i is the weight of the ith quaternion. + * In this case, all quaternions are equally weighted (i.e. w_i = 1) + */ + + Eigen::Matrix4d M = Eigen::Matrix4d::Zero(); + + for(const Eigen::Quaterniond& q : quaterns) + { + M += q.coeffs() * q.coeffs().transpose(); + } + + // Eigenvectors,values should be strictly real + Eigen::EigenSolver E(M, true); + + // Each column of 4x4 vectors is an eigenvector; desired mean has max + Eigen::Index idx_max_ev; // Index of the largest eigenvalue + + //find maximium eigenvalue, and store its index in max_evi + E.eigenvalues().real().maxCoeff(&idx_max_ev); + + Eigen::Quaterniond q; + q.coeffs() << E.eigenvectors().real().col(idx_max_ev); + + assert(std::isnan(q.w()) == false && + std::isnan(q.x()) == false && + std::isnan(q.y()) == false && + std::isnan(q.z()) == false); + + return q; +}; + +QuaternionStats computeQuaternionStats(const std::vector &quaternions) +{ + QuaternionStats q_stats; + q_stats.mean = computeQuaternionMean(quaternions); + + double q_var = 0.0; + for (const Eigen::Quaterniond &q : quaternions) + { + q_var += std::pow(q_stats.mean.angularDistance(q), 2.0); + } + q_var /= static_cast(quaternions.size()); + q_stats.stdev = std::sqrt(q_var); + + return q_stats; +} + +PnPNoiseStat qualifyNoise2D(const std::vector& params) +{ + PnPNoiseStat output; + std::size_t count = params.size(); + + std::vector solution_transforms; + solution_transforms.reserve(count); + + std::vector translations; + translations.reserve(count); + + std::vector orientations; + orientations.reserve(count); + + namespace ba = boost::accumulators; + ba::accumulator_set> x_acc; + ba::accumulator_set> y_acc; + ba::accumulator_set> z_acc; + + for (auto& prob : params) + { + rct_optimizations::PnPResult result; + + result = rct_optimizations::optimize(prob); + + if (result.converged) + { + //we will save the full result here for debugging purposes + solution_transforms.push_back(result.camera_to_target); + translations.push_back(solution_transforms.back().translation()); + + x_acc(result.camera_to_target.translation()(0)); + y_acc(result.camera_to_target.translation()(1)); + z_acc(result.camera_to_target.translation()(2)); + + orientations.push_back(Eigen::Quaterniond(result.camera_to_target.rotation())); + } + } + + output.p_stat.mean << ba::mean(x_acc), ba::mean(y_acc), ba::mean(z_acc); + output.p_stat.stdev << std::sqrt(ba::variance(x_acc)), std::sqrt(ba::variance(y_acc)), std::sqrt(ba::variance(z_acc)); + output.q_stat = computeQuaternionStats(orientations); + + return output; +} + +PnPNoiseStat qualifyNoise3D(const std::vector& params) +{ + PnPNoiseStat output; + std::size_t count = params.size(); + + std::vector solution_transforms; + solution_transforms.reserve(count); + + std::vector translations; + translations.reserve(count); + + std::vector orientations; + orientations.reserve(count); + + namespace ba = boost::accumulators; + ba::accumulator_set> x_acc; + ba::accumulator_set> y_acc; + ba::accumulator_set> z_acc; + + for (auto& prob : params) + { + rct_optimizations::PnPResult result; + + result = rct_optimizations::optimize(prob); + + if (result.converged) + { + //we will save the full result here for debugging purposes + solution_transforms.push_back(result.camera_to_target); + translations.push_back(solution_transforms.back().translation()); + + x_acc(result.camera_to_target.translation()(0)); + y_acc(result.camera_to_target.translation()(1)); + z_acc(result.camera_to_target.translation()(2)); + + orientations.push_back(Eigen::Quaterniond(result.camera_to_target.rotation())); + } + } + + output.p_stat.mean << ba::mean(x_acc), ba::mean(y_acc), ba::mean(z_acc); + output.p_stat.stdev << std::sqrt(ba::variance(x_acc)), std::sqrt(ba::variance(y_acc)), std::sqrt(ba::variance(z_acc)); + output.q_stat = computeQuaternionStats(orientations); + + return output; +} + +}//rct_optimizations diff --git a/rct_optimizations/src/validation/noise_qualifier.cpp b/rct_optimizations/src/validation/noise_qualifier.cpp deleted file mode 100644 index 82398f7a..00000000 --- a/rct_optimizations/src/validation/noise_qualifier.cpp +++ /dev/null @@ -1,204 +0,0 @@ -#include -#include -#include -#include -#include -#include - -//accumulators -#include -#include -#include -#include -#include - -namespace rct_optimizations -{ - -RotationStat FindQuaternionMean(const std::vector& quaterns) -{ - /* Mean quaternion is found using method described by Markley et al: Quaternion Averaging - * https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf - * All quaternions are equally weighted - * - * Negative quaternions are left as such - */ - - std::size_t count = quaterns.size(); - std::vector orientations; - for (std::size_t i=0; i < quaterns.size(); ++i) - { - orientations.push_back(quaterns[i].coeffs()); - } - - RotationStat q; - - //Accumulator Matrix - Eigen::Matrix4d A = Eigen::Matrix4d::Zero(); - - for (std::size_t i = 0; i <= count; ++i) - { - //rank 1 update of 'A', the accumulator matrix, with the u,v = q,qT - Eigen::Matrix4d temp = (orientations[i] * orientations[i].transpose()); - A += temp; - } - - //scale A - A = (1.0/double(count)) * A; - - //Eigenvectors,values should be strictly real - Eigen::EigenSolver E(A, true); - - //Each column of 4x4 vectors is an eigenvector; desired mean has max - //eigenvalue with index stored in max_evi - Eigen::Index max_evi, one; - - //find maximium eigenvalue, and store its index in max_evi - E.eigenvalues().real().maxCoeff(&max_evi, &one); - Eigen::Vector4d mean = E.eigenvectors().real().col(max_evi); - - q.qx.mean = mean[0]; - q.qy.mean = mean[1]; - q.qz.mean = mean[2]; - q.qw.mean = mean[3]; - - assert(std::isnan(q.qx.mean) == false && - std::isnan(q.qy.mean) == false && - std::isnan(q.qz.mean) == false && - std::isnan(q.qw.mean) == false); - - //Manually calculate standard deviations from mean - Eigen::Array4d std_dev = Eigen::Array4d::Zero(); - Eigen::Array4d sum = Eigen::Array4d::Zero(); - for (std::size_t i =0; i< count; ++i) - { - //absolute value taken because a quaternion is equal to its opposite - Eigen::Array4d term = orientations[i].cwiseAbs() - mean.cwiseAbs(); - term = term.square(); - sum+= term; - } - - std_dev = sum/double(count); - std_dev = std_dev.sqrt(); - - q.qx.std_dev = std_dev[0]; - q.qy.std_dev = std_dev[1]; - q.qz.std_dev = std_dev[2]; - q.qw.std_dev = std_dev[3]; - - return q; -}; - - - PnPNoiseStat qualifyNoise2D(const std::vector& params) - { - - PnPNoiseStat output; - std::size_t count = params.size(); - - std::vector solution_transforms; - solution_transforms.reserve(count); - - std::vector translations; - translations.reserve(count); - - std::vector orientations; - orientations.reserve(count); - - using namespace boost::accumulators; - accumulator_set> x_acc; - accumulator_set> y_acc; - accumulator_set> z_acc; - - - for (auto& prob : params) - { - rct_optimizations::PnPResult result; - - result = rct_optimizations::optimize(prob); - - if (result.converged) - { - //we will save the full result here for debugging purposes - solution_transforms.push_back(result.camera_to_target); - translations.push_back(solution_transforms.back().translation()); - - x_acc(result.camera_to_target.translation()(0)); - y_acc(result.camera_to_target.translation()(1)); - z_acc(result.camera_to_target.translation()(2)); - - orientations.push_back(Eigen::Quaterniond(result.camera_to_target.rotation())); - } - } - - output.x.mean = boost::accumulators::mean(x_acc); - output.y.mean = boost::accumulators::mean(y_acc); - output.z.mean = boost::accumulators::mean(z_acc); - output.x.std_dev = sqrt(boost::accumulators::variance(x_acc)); - output.y.std_dev = sqrt(boost::accumulators::variance(y_acc)); - output.z.std_dev = sqrt(boost::accumulators::variance(z_acc)); - - - RotationStat quater = FindQuaternionMean(orientations); - output.q = quater; - - //Output: mean & standard deviation of x,y,z,qx,qy,qz,qw - return output; -} - - PnPNoiseStat qualifyNoise3D(const std::vector& params) - { - - PnPNoiseStat output; - std::size_t count = params.size(); - - std::vector solution_transforms; - solution_transforms.reserve(count); - - std::vector translations; - translations.reserve(count); - - std::vector orientations; - orientations.reserve(count); - - using namespace boost::accumulators; - accumulator_set> x_acc; - accumulator_set> y_acc; - accumulator_set> z_acc; - - - for (auto& prob : params) - { - rct_optimizations::PnPResult result; - - result = rct_optimizations::optimize(prob); - - if (result.converged) - { - //we will save the full result here for debugging purposes - solution_transforms.push_back(result.camera_to_target); - translations.push_back(solution_transforms.back().translation()); - - x_acc(result.camera_to_target.translation()(0)); - y_acc(result.camera_to_target.translation()(1)); - z_acc(result.camera_to_target.translation()(2)); - - orientations.push_back(Eigen::Quaterniond(result.camera_to_target.rotation())); - } - } - - output.x.mean = boost::accumulators::mean(x_acc); - output.y.mean = boost::accumulators::mean(y_acc); - output.z.mean = boost::accumulators::mean(z_acc); - output.x.std_dev = sqrt(boost::accumulators::variance(x_acc)); - output.y.std_dev = sqrt(boost::accumulators::variance(y_acc)); - output.z.std_dev = sqrt(boost::accumulators::variance(z_acc)); - - RotationStat quater = FindQuaternionMean(orientations); - output.q = quater; - - //Output: mean & standard deviation of x,y,z,qx,qy,qz,qw - return output; -} - -}//rct_optimizations diff --git a/rct_optimizations/test/noise_qualification_utest.cpp b/rct_optimizations/test/noise_qualification_utest.cpp index d7d05609..6f0c446a 100644 --- a/rct_optimizations/test/noise_qualification_utest.cpp +++ b/rct_optimizations/test/noise_qualification_utest.cpp @@ -4,470 +4,340 @@ #include #include #include -#include +#include #include -//tolerance for oreintation difference, in radians -#define ANGULAR_THRESHOLD 8*M_PI/180 - using namespace rct_optimizations; +static const unsigned TARGET_ROWS = 10; +static const unsigned TARGET_COLS = 14; +static const double SPACING = 0.025; +static const double CAMERA_STANDOFF = 0.5; + TEST(NoiseTest, QuatMeanTest) { - /*This test validate the method used to find the mean quaternion - * in noise_qualifier.cpp - */ - //base quaternion - Eigen::Quaterniond quat_1 (0,1,0,0); - Eigen::Quaterniond quat_2 (0,0,1,0); + Eigen::Quaterniond q_mean(1, 0, 0, 0); - std::vector poses = {quat_1, quat_2}; + // Make a lot of quaternions randomly oriented about the x-axis + std::mt19937 mt_rand(std::random_device{}()); + double stdev = M_PI / 8.0; + std::normal_distribution dist(0.0, stdev); - //average new quats - RotationStat r = FindQuaternionMean(poses); + std::size_t n = 1000; + std::vector random_q; + random_q.reserve(n); + for (std::size_t i = 0; i < n; ++i) + { + random_q.push_back(q_mean * Eigen::AngleAxisd(dist(mt_rand), Eigen::Vector3d::UnitX())); + } - Eigen::Quaterniond mean_quat1 (r.qw.mean, r.qx.mean, r.qy.mean, r.qz.mean); + //average new quats + QuaternionStats q_stats = computeQuaternionStats(random_q); //The two quaternions are 2 pi rad apart, so the mean should be ~PI away from both - EXPECT_LT(mean_quat1.angularDistance(quat_1) - M_PI, ANGULAR_THRESHOLD); - EXPECT_LT(mean_quat1.angularDistance(quat_2) - M_PI, ANGULAR_THRESHOLD); + EXPECT_LT(q_mean.angularDistance(q_stats.mean), 1.0 * M_PI / 180.0); + EXPECT_LT(std::abs(stdev - q_stats.stdev), 1.0 * M_PI / 180.0); } -TEST(NoiseTest, 2DPerfectTest) +class NoiseQualification2D : public ::testing::Test { - //make target - test::Target target(4, 4, 0.025); + public: + NoiseQualification2D() + : target(TARGET_ROWS, TARGET_COLS, SPACING) + , camera(test::makeKinectCamera()) + , target_to_camera(Eigen::Isometry3d::Identity()) + , mt_rand(std::random_device{}()) + { + // Put the camera over the center of the target facing normal to the target + double x = static_cast(TARGET_ROWS - 1) * SPACING / 2.0; + double y = static_cast(TARGET_COLS - 1) * SPACING / 2.0; + target_to_camera.translate(Eigen::Vector3d(x, y, CAMERA_STANDOFF)); + target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + } + + Correspondence2D3D::Set createCorrespondences() + { + Correspondence2D3D::Set out; + EXPECT_NO_THROW(out = test::getCorrespondences(target_to_camera, + Eigen::Isometry3d::Identity(), + camera, + target, + true);); + return out; + } - //Load camera intrinsics - //camera is a kinect - auto camera = test::makeKinectCamera(); + Correspondence2D3D::Set createNoisyCorrespondences(double mean, double stdev) + { + std::normal_distribution dist(mean, stdev); + Correspondence2D3D::Set out = createCorrespondences(); + for (auto &correspondence : out) + { + Eigen::Vector2d noise; + noise << dist(mt_rand), dist(mt_rand); + correspondence.in_image += noise; + } + return out; + } + + test::Target target; + test::Camera camera; + Eigen::Isometry3d target_to_camera; + Correspondence2D3D::Set correspondences; + std::mt19937 mt_rand; +}; +TEST_F(NoiseQualification2D, PerfectData) +{ //reserve observations - std::size_t obs_cnt = 35; + const std::size_t obs_cnt = 35; std::vector ideal_problem_set; ideal_problem_set.reserve(obs_cnt); - Eigen::Isometry3d target_loc = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d camera_loc = Eigen::Isometry3d::Identity(); - - camera_loc.translate(Eigen::Vector3d(0.0,0.0,1.0)); - camera_loc.rotate(Eigen::AngleAxisd(M_PI,Eigen::Vector3d::UnitX())); - - Eigen::Quaterniond q_ver; - q_ver = camera_loc.rotation(); - //create observations + Eigen::Isometry3d expected_pose = target_to_camera.inverse(); for (std::size_t i = 0; i < obs_cnt; ++i) { - PnPProblem instance; - //guess inital position - instance.camera_to_target_guess= camera_loc; - instance.intr = camera.intr; - - Correspondence2D3D::Set corr; - EXPECT_NO_THROW - ( - corr = getCorrespondences(camera_loc, - target_loc, - camera, - target, - true); - ); - - instance.correspondences = corr; - ideal_problem_set.push_back(instance); + PnPProblem problem; + problem.camera_to_target_guess = expected_pose; + problem.intr = camera.intr; + problem.correspondences = createCorrespondences(); + ideal_problem_set.push_back(problem); } - PnPNoiseStat output = rct_optimizations::qualifyNoise2D(ideal_problem_set); - - EXPECT_LT(output.x.std_dev, 1.0e-14); - EXPECT_LT(output.y.std_dev, 1.0e-14); - EXPECT_LT(output.z.std_dev, 1.0e-14); - EXPECT_LT(output.q.qx.std_dev, 1.0e-14); - EXPECT_LT(output.q.qy.std_dev, 1.0e-14); - EXPECT_LT(output.q.qz.std_dev, 1.0e-14); - EXPECT_LT(output.q.qw.std_dev, 1.0e-14); - - //Absolute value of quaternion is taken, since quaternions equal their oppoisite - EXPECT_LT(abs(output.x.mean - camera_loc.translation()(0)), 1.0e-14); - EXPECT_LT(abs(output.y.mean - camera_loc.translation()(1)), 1.0e-14); - EXPECT_LT(abs(output.z.mean - camera_loc.translation()(2)), 1.0e-14); - EXPECT_LT(abs(output.q.qx.mean) - abs(q_ver.x()), 1.0e-14); - EXPECT_LT(abs(output.q.qy.mean) - abs(q_ver.y()), 1.0e-14); - EXPECT_LT(abs(output.q.qz.mean) - abs(q_ver.z()), 1.0e-14); - EXPECT_LT(abs(output.q.qw.mean) - abs(q_ver.w()), 1.0e-14); + PnPNoiseStat results = rct_optimizations::qualifyNoise2D(ideal_problem_set); + + EXPECT_TRUE(results.p_stat.mean.isApprox(expected_pose.translation())); + EXPECT_LT(results.p_stat.stdev.norm(), 1.0e-15); + EXPECT_LT(results.q_stat.mean.angularDistance(Eigen::Quaterniond(expected_pose.linear())), 1.0e-15); + EXPECT_LT(results.q_stat.stdev, 1.0e-15); } -TEST(NoiseTest, 2DNoiseTest) +TEST_F(NoiseQualification2D, NoisyData) { - //make target - test::Target target(4, 4, 0.025); - - //Load camera intrinsics - //camera is a kinect - auto camera = test::makeKinectCamera(); - //reserve observations std::size_t obs_cnt = 35; std::vector perturbed_problem_set; perturbed_problem_set.reserve(obs_cnt); - Eigen::Isometry3d target_loc = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d camera_loc = Eigen::Isometry3d::Identity(); - - camera_loc.translate(Eigen::Vector3d(0.0,0.0,1.0)); - camera_loc.rotate(Eigen::AngleAxisd(M_PI,Eigen::Vector3d::UnitX())); - - Eigen::Quaterniond q_ver; - q_ver = camera_loc.rotation(); - //add noise boilerplate const double mean = 0.0; - const double stddev = 1.0; - std::random_device rd{}; - std::mt19937 generator{rd()}; - std::normal_distribution dist(mean, stddev); + const double stdev = 2.0; //create observations + Eigen::Isometry3d expected_pose = target_to_camera.inverse(); for (std::size_t i = 0; i < obs_cnt; ++i) { - PnPProblem instance; - //guess inital position - instance.camera_to_target_guess= camera_loc; - instance.intr = camera.intr; - - Correspondence2D3D::Set corr; - EXPECT_NO_THROW - ( - corr = getCorrespondences(camera_loc, - target_loc, - camera, - target, - true); - ); - - instance.correspondences = corr; - - for (std::size_t j = 0; j < instance.correspondences.size(); ++j) - { - double wobblex = dist(generator); - double wobbley = dist(generator); - instance.correspondences[j].in_image(0) += wobblex; - instance.correspondences[j].in_image(1) += wobbley; - } - - perturbed_problem_set.push_back(instance); + PnPProblem problem; + problem.camera_to_target_guess = expected_pose; + problem.intr = camera.intr; + problem.correspondences = createNoisyCorrespondences(mean, stdev); + + perturbed_problem_set.push_back(problem); } - PnPNoiseStat output = rct_optimizations::qualifyNoise2D(perturbed_problem_set); + PnPNoiseStat results = rct_optimizations::qualifyNoise2D(perturbed_problem_set); - //Project true target position into the camera - double uv[2]; - Eigen::Vector3d second_target_loc = camera_loc.translation(); - rct_optimizations::projectPoint(camera.intr, second_target_loc.data(), uv); + // Project the mean camera to target translation into the camera + Eigen::Vector2d uv = projectPoint(camera.intr, results.p_stat.mean); - EXPECT_LT(output.x.std_dev, 1.5 * stddev); - EXPECT_LT(output.y.std_dev, 1.5 * stddev); - EXPECT_LT(output.z.std_dev, 1.5 * stddev); + // Take the differential of the camera projection equations to estimate the change in [x,y,z] that is expected from applying camera image noise of [stdev, stdev] + // u = fx * (x / z) + cx --> dx = du * (z / fx) + // v = fy * (y / z) + cy --> dy = du * (z / fy) + // z = fx * x / (u - cx) --> dz = du * -1.0 * (fx * x) / (u - cx)^2 + Eigen::Vector3d delta; + delta.x() = stdev * results.p_stat.mean.z() / camera.intr.fx(); + delta.y() = stdev * results.p_stat.mean.z() / camera.intr.fy(); + delta.z() = stdev * -1.0 * camera.intr.fx() * results.p_stat.mean.x() + / std::pow((uv(0) - camera.intr.cx()), 2.0); - //Absolute value of quaternion is taken, since quaternions equal their oppoisite - EXPECT_LT(Eigen::Quaterniond(abs(output.q.qw.mean), abs(output.q.qx.mean), abs(output.q.qy.mean), abs(output.q.qz.mean)).angularDistance(q_ver), ANGULAR_THRESHOLD); + EXPECT_LT(results.p_stat.stdev.norm(), delta.norm()); + EXPECT_TRUE(results.p_stat.mean.isApprox(expected_pose.translation(), delta.norm())); - EXPECT_LT(abs(output.x.mean - camera_loc.translation()(0)), (camera_loc.translation()(2)/camera.intr.fx()) * stddev); - EXPECT_LT(abs(output.y.mean - camera_loc.translation()(1)), (camera_loc.translation()(2)/camera.intr.fy()) * stddev); - //Max is taken in case the target location is is exactly correct & threshold goes to 0 - EXPECT_LT(abs(output.z.mean - camera_loc.translation()(2)), std::max(1.5*stddev, camera.intr.fx() * camera_loc.translation()(0) * std::sqrt(uv[0] - camera.intr.cx()) * stddev)); + // Expect the mean quaternion to be within 1 sigma of the expected orientation + EXPECT_LT(results.q_stat.mean.angularDistance(Eigen::Quaterniond(expected_pose.linear())), + results.q_stat.stdev); } -TEST(NoiseTest, 2DTwistNoiseTest) +TEST_F(NoiseQualification2D, NoisyDataPerturbedGuess) { - //make target - test::Target target(4, 4, 0.025); - - //Load camera intrinsics - //camera is a kinect - auto camera = test::makeKinectCamera(); - //reserve observations std::size_t obs_cnt = 35; std::vector perturbed_problem_set; perturbed_problem_set.reserve(obs_cnt); - Eigen::Isometry3d target_loc = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d camera_loc = Eigen::Isometry3d::Identity(); - - camera_loc.translate(Eigen::Vector3d(0.0,0.0,1.0)); - camera_loc.rotate(Eigen::AngleAxisd(M_PI,Eigen::Vector3d::UnitX())); - camera_loc.rotate(Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ())); - - Eigen::Quaterniond q_ver; - q_ver = camera_loc.rotation(); - //add noise boilerplate const double mean = 0.0; - const double stddev = 1.0; - std::random_device rd{}; - std::mt19937 generator{rd()}; - std::normal_distribution dist(mean, stddev); + const double stdev = 2.0; //create observations + Eigen::Isometry3d expected_pose = target_to_camera.inverse(); for (std::size_t i = 0; i < obs_cnt; ++i) { - PnPProblem instance; - //guess inital position - instance.camera_to_target_guess= camera_loc; - instance.intr = camera.intr; - - Correspondence2D3D::Set corr; - EXPECT_NO_THROW - ( - corr = getCorrespondences(camera_loc, - target_loc, - camera, - target, - true); - ); - - instance.correspondences = corr; - - for (std::size_t j = 0; j < instance.correspondences.size(); ++j) - { - double wobblex = dist(generator); - double wobbley = dist(generator); - instance.correspondences[j].in_image(0) += wobblex; - instance.correspondences[j].in_image(1) += wobbley; - } - - perturbed_problem_set.push_back(instance); - } + PnPProblem problem; + problem.camera_to_target_guess = test::perturbPose(expected_pose, 0.01, 0.01); + problem.intr = camera.intr; + problem.correspondences = createNoisyCorrespondences(mean, stdev); - PnPNoiseStat output = rct_optimizations::qualifyNoise2D(perturbed_problem_set); + perturbed_problem_set.push_back(problem); + } + PnPNoiseStat results = rct_optimizations::qualifyNoise2D(perturbed_problem_set); - //find true target position in camera - //target position relative to camera - double uv[2]; - Eigen::Vector3d second_target_loc = camera_loc.translation(); - rct_optimizations::projectPoint(camera.intr, second_target_loc.data(), uv); + // Project the mean camera to target translation into the camera + Eigen::Vector2d uv = projectPoint(camera.intr, results.p_stat.mean); + // Take the differential of the camera projection equations to estimate the change in [x,y,z] that is expected from applying camera image noise of [stdev, stdev] + // u = fx * (x / z) + cx --> dx = du * (z / fx) + // v = fy * (y / z) + cy --> dy = du * (z / fy) + // z = fx * x / (u - cx) --> dz = du * -1.0 * (fx * x) / (u - cx)^2 + Eigen::Vector3d delta; + delta.x() = stdev * results.p_stat.mean.z() / camera.intr.fx(); + delta.y() = stdev * results.p_stat.mean.z() / camera.intr.fy(); + delta.z() = stdev * -1.0 * camera.intr.fx() * results.p_stat.mean.x() + / std::pow((uv(0) - camera.intr.cx()), 2.0); - EXPECT_TRUE(output.x.std_dev < 1.5 * stddev); - EXPECT_TRUE(output.y.std_dev < 1.5 * stddev); - EXPECT_TRUE(output.z.std_dev < 1.5 * stddev); - EXPECT_LT(Eigen::Quaterniond(output.q.qw.mean, output.q.qx.mean, output.q.qy.mean, output.q.qz.mean).angularDistance(q_ver), ANGULAR_THRESHOLD); + EXPECT_LT(results.p_stat.stdev.norm(), delta.norm()); + EXPECT_TRUE(results.p_stat.mean.isApprox(expected_pose.translation(), delta.norm())); - //Absolute value of quaternion is taken, since quaternions equal their oppoisite - EXPECT_LT(abs(output.x.mean - camera_loc.translation()(0)), (camera_loc.translation()(2)/camera.intr.fx()) * stddev); - EXPECT_LT(abs(output.y.mean - camera_loc.translation()(1)), (camera_loc.translation()(2)/camera.intr.fy()) * stddev); - EXPECT_LT(abs(output.z.mean - camera_loc.translation()(2)), std::max(1.5*stddev, camera.intr.fx() * camera_loc.translation()(0) * std::sqrt(uv[0] - camera.intr.cx()) * stddev)); + // Expect the mean quaternion to be within 1 sigma of the expected orientation + EXPECT_LT(results.q_stat.mean.angularDistance(Eigen::Quaterniond(expected_pose.linear())), + results.q_stat.stdev); } -TEST(NoiseTest, 3DPerfectTest) +class NoiseQualification3D : public ::testing::Test { - //make target - test::Target target(4, 4, 0.025); + public: + NoiseQualification3D() + : target(test::Target(TARGET_ROWS, TARGET_COLS, SPACING)) + , target_to_camera(Eigen::Isometry3d::Identity()) + , mt_rand(std::random_device{}()) + { + // Put the camera over the center of the target facing normal to the target + double x = static_cast(TARGET_ROWS - 1) * SPACING / 2.0; + double y = static_cast(TARGET_COLS - 1) * SPACING / 2.0; + target_to_camera.translate(Eigen::Vector3d(x, y, CAMERA_STANDOFF)); + target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + } + + Correspondence3D3D::Set createCorrespondences() + { + Correspondence3D3D::Set out; + EXPECT_NO_THROW( + out = test::getCorrespondences(target_to_camera, Eigen::Isometry3d::Identity(), target);); + + return out; + } + + Correspondence3D3D::Set createNoisyCorrespondences(const double mean, const double stdev) + { + Correspondence3D3D::Set out = createCorrespondences(); + std::normal_distribution dist(mean, stdev); + + for (auto &corr : out) + { + Eigen::Vector3d noise; + noise << dist(mt_rand), dist(mt_rand), dist(mt_rand); + corr.in_image += noise; + } + + return out; + } + test::Target target; + Eigen::Isometry3d target_to_camera; + std::mt19937 mt_rand; +}; +TEST_F(NoiseQualification3D, PerfectData) +{ //reserve observations std::size_t obs_cnt = 35; std::vector ideal_problem_set; ideal_problem_set.reserve(obs_cnt); - Eigen::Isometry3d target_loc = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d camera_loc = Eigen::Isometry3d::Identity(); - - camera_loc.translate(Eigen::Vector3d(0.0,0.0,1.0)); - camera_loc.rotate(Eigen::AngleAxisd(M_PI,Eigen::Vector3d::UnitX())); - - Eigen::Quaterniond q_ver; - q_ver = camera_loc.rotation(); - //create observations + Eigen::Isometry3d expected_pose = target_to_camera.inverse(); for (std::size_t i = 0; i < obs_cnt; ++i) { - PnPProblem3D instance; - //start with a perfect guess - instance.camera_to_target_guess = camera_loc; - - Correspondence3D3D::Set corr; - EXPECT_NO_THROW( - corr = getCorrespondences(camera_loc, - target_loc, - target); - ); - - instance.correspondences = corr; - ideal_problem_set.push_back(instance); + PnPProblem3D problem; + problem.camera_to_target_guess = expected_pose; + problem.correspondences = createCorrespondences(); + ideal_problem_set.push_back(problem); } - PnPNoiseStat output = rct_optimizations::qualifyNoise3D(ideal_problem_set); - - EXPECT_LT(output.x.std_dev, 1.0e-14); - EXPECT_LT(output.y.std_dev, 1.0e-14); - EXPECT_LT(output.z.std_dev, 1.0e-14); - //Rotational accuracy is less than positional, possibly because orientation is not optimized directly - EXPECT_LT(output.q.qx.std_dev, ANGULAR_THRESHOLD); - EXPECT_LT(output.q.qy.std_dev, ANGULAR_THRESHOLD); - EXPECT_LT(output.q.qz.std_dev, ANGULAR_THRESHOLD); - EXPECT_LT(output.q.qw.std_dev, ANGULAR_THRESHOLD); - - //Absolute value of quaternion is taken, since quaternions equal their oppoisite - EXPECT_LT(abs(output.x.mean - camera_loc.translation()(0)), 1.0e-14); - EXPECT_LT(abs(output.y.mean - camera_loc.translation()(1)), 1.0e-14); - EXPECT_LT(abs(output.z.mean - camera_loc.translation()(2)), 1.0e-14); - EXPECT_LT(abs(output.q.qx.mean) - abs(q_ver.x()), ANGULAR_THRESHOLD); - EXPECT_LT(abs(output.q.qy.mean) - abs(q_ver.y()), ANGULAR_THRESHOLD); - EXPECT_LT(abs(output.q.qz.mean) - abs(q_ver.z()), ANGULAR_THRESHOLD); - EXPECT_LT(abs(output.q.qw.mean) - abs(q_ver.w()), ANGULAR_THRESHOLD ); + PnPNoiseStat result = rct_optimizations::qualifyNoise3D(ideal_problem_set); + + EXPECT_TRUE(result.p_stat.mean.isApprox(expected_pose.translation())); + EXPECT_LT(result.p_stat.stdev.norm(), 1.0e-15); + EXPECT_LT(result.q_stat.mean.angularDistance(Eigen::Quaterniond(expected_pose.linear())), 1.0e-15); + EXPECT_LT(result.q_stat.stdev, 1.0e-15); } -TEST(NoiseTest, 3DNoiseTest) +TEST_F(NoiseQualification3D, NoisyData) { - //make target - test::Target target(4, 4, 0.025); - //reserve observations std::size_t obs_cnt = 35; - std::vector perturbed_problem_set; - perturbed_problem_set.reserve(obs_cnt); - - Eigen::Isometry3d target_loc = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d camera_loc = Eigen::Isometry3d::Identity(); - - camera_loc.translate(Eigen::Vector3d(0.05,0.01,1.0)); - camera_loc.rotate(Eigen::AngleAxisd(M_PI,Eigen::Vector3d::UnitX())); - - Eigen::Quaterniond q_ver; - q_ver = camera_loc.rotation(); + std::vector ideal_problem_set; + ideal_problem_set.reserve(obs_cnt); - //Noise boilerplate - const double mean = 0.0; - //Smaller std dev magnitude bc in meters, not pixels - const double stddev = 0.005; - std::random_device rd{}; - std::mt19937 generator{rd()}; - std::normal_distribution dist(mean, stddev); + double mean = 0.0; + double stdev = 0.005; //create observations + Eigen::Isometry3d expected_pose = target_to_camera.inverse(); for (std::size_t i = 0; i < obs_cnt; ++i) { - PnPProblem3D instance; - - //start with a perfect guess - instance.camera_to_target_guess = camera_loc; - - Correspondence3D3D::Set corr; - - EXPECT_NO_THROW( - corr = getCorrespondences(camera_loc, - target_loc, - target); - ); - - instance.correspondences = corr; - - - //now add noise to correspondences - for (std::size_t j = 0; j < instance.correspondences.size(); ++j) - { - double wobblex = dist(generator); - double wobbley = dist(generator); - instance.correspondences[j].in_image(0) += wobblex; - instance.correspondences[j].in_image(1) += wobbley; - } - - perturbed_problem_set.push_back(instance); + PnPProblem3D problem; + problem.camera_to_target_guess = expected_pose; + problem.correspondences = createNoisyCorrespondences(mean, stdev); + ideal_problem_set.push_back(problem); } - PnPNoiseStat output = rct_optimizations::qualifyNoise3D(perturbed_problem_set); + PnPNoiseStat result = rct_optimizations::qualifyNoise3D(ideal_problem_set); - EXPECT_LT(output.x.std_dev, 1.5 * stddev); - EXPECT_LT(output.y.std_dev, 1.5 * stddev); - EXPECT_LT(output.z.std_dev, 1.5 * stddev); - EXPECT_LT(Eigen::Quaterniond(output.q.qw.mean, output.q.qx.mean, output.q.qy.mean, output.q.qz.mean).angularDistance(q_ver), ANGULAR_THRESHOLD); + // Create a variable for the magnitude of the standard deviation since it was applied in all three directions + double stdev_norm = std::sqrt(3) * stdev; - //Absolute value of quaternion is taken, since quaternions equal their oppoisite - //Comparing to Standard Deviation, because 3d camera does not have explicit instrinsics - EXPECT_LT(-1.0 * output.x.mean - camera_loc.translation()(0), 1.5 * stddev); - EXPECT_LT(-1.0 * output.y.mean - camera_loc.translation()(1), 1.5 * stddev); - EXPECT_LT(abs(output.z.mean - camera_loc.translation()(2)), 1.5 * stddev); + EXPECT_TRUE(result.p_stat.mean.isApprox(expected_pose.translation(), stdev_norm)); + EXPECT_LT(result.p_stat.stdev.norm(), stdev_norm); + // Expect that the average quaternion is within one standard deviation of the expected orientation + EXPECT_LT(result.q_stat.mean.angularDistance(Eigen::Quaterniond(expected_pose.linear())), result.q_stat.stdev); } -TEST(NoiseTest, 3DTwistNoiseTest) +TEST_F(NoiseQualification3D, NoisyDataPerturbedGuess) { - //make target - test::Target target(4, 4, 0.025); - //reserve observations std::size_t obs_cnt = 35; - std::vector perturbed_problem_set; - perturbed_problem_set.reserve(obs_cnt); - - Eigen::Isometry3d target_loc = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d camera_loc = Eigen::Isometry3d::Identity(); - - camera_loc.translate(Eigen::Vector3d(0.01,0.01,1.0)); - camera_loc.rotate(Eigen::AngleAxisd(M_PI,Eigen::Vector3d::UnitX())); - camera_loc.rotate(Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ())); + std::vector ideal_problem_set; + ideal_problem_set.reserve(obs_cnt); - Eigen::Quaterniond q_ver; - q_ver = camera_loc.rotation(); - - //Noise boilerplate - const double mean = 0.0; - //Smaller std dev magnitude bc in meters, not pixels - const double stddev = 0.005; - std::random_device rd{}; - std::mt19937 generator{rd()}; - std::normal_distribution dist(mean, stddev); + double mean = 0.0; + double stdev = 0.005; //create observations + Eigen::Isometry3d expected_pose = target_to_camera.inverse(); for (std::size_t i = 0; i < obs_cnt; ++i) { - PnPProblem3D instance; - - //start with a perfect guess - instance.camera_to_target_guess = camera_loc; - - Correspondence3D3D::Set corr; - - EXPECT_NO_THROW( - corr = getCorrespondences(camera_loc, - target_loc, - target); - ); - - instance.correspondences = corr; - - - //now add noise to correspondences - for (std::size_t j = 0; j < instance.correspondences.size(); ++j) - { - double wobblex = dist(generator); - double wobbley = dist(generator); - instance.correspondences[j].in_image(0) += wobblex; - instance.correspondences[j].in_image(1) += wobbley; - } - - perturbed_problem_set.push_back(instance); + PnPProblem3D problem; + problem.camera_to_target_guess = test::perturbPose(expected_pose, 0.01, 0.01); + problem.correspondences = createNoisyCorrespondences(mean, stdev); + ideal_problem_set.push_back(problem); } - PnPNoiseStat output = rct_optimizations::qualifyNoise3D(perturbed_problem_set); + PnPNoiseStat result = rct_optimizations::qualifyNoise3D(ideal_problem_set); - EXPECT_TRUE(output.x.std_dev < 1.5 * stddev); - EXPECT_TRUE(output.y.std_dev < 1.5 * stddev); - EXPECT_TRUE(output.z.std_dev < 1.5 * stddev); - EXPECT_LT(Eigen::Quaterniond(output.q.qw.mean, output.q.qx.mean, output.q.qy.mean, output.q.qz.mean).angularDistance(q_ver), ANGULAR_THRESHOLD); + // Create a variable for the magnitude of the standard deviation since it was applied in all three directions + double stdev_norm = std::sqrt(3) * stdev; - //Absolute value of quaternion is taken, since quaternions equal their oppoisite - EXPECT_LT(abs(output.x.mean - camera_loc.translation()(0)), 1.5 * stddev);; - EXPECT_LT(abs(output.y.mean - camera_loc.translation()(1)), 1.5 * stddev); - EXPECT_LT(abs(output.z.mean - camera_loc.translation()(2)), 1.5 * stddev); + EXPECT_TRUE(result.p_stat.mean.isApprox(expected_pose.translation(), stdev_norm)); + EXPECT_LT(result.p_stat.stdev.norm(), stdev_norm); + // Expect that the average quaternion is within one standard deviation of the expected orientation + EXPECT_LT(result.q_stat.mean.angularDistance(Eigen::Quaterniond(expected_pose.linear())), result.q_stat.stdev); } int main(int argc, char **argv)