From eb726dc7085defa20699b9aa2f6e235acfa470b1 Mon Sep 17 00:00:00 2001 From: mripperger Date: Fri, 14 Aug 2020 13:47:51 -0500 Subject: [PATCH 1/7] WIP add kinematic calibration example --- rct_examples/CMakeLists.txt | 9 + .../src/tools/kinematic_calibration.cpp | 307 ++++++++++++++++++ 2 files changed, 316 insertions(+) create mode 100644 rct_examples/src/tools/kinematic_calibration.cpp diff --git a/rct_examples/CMakeLists.txt b/rct_examples/CMakeLists.txt index 140e8a3c..f1d98e3f 100644 --- a/rct_examples/CMakeLists.txt +++ b/rct_examples/CMakeLists.txt @@ -111,6 +111,15 @@ set_target_properties(${PROJECT_NAME}_noise_qualification_2d PROPERTIES OUTPUT_N add_dependencies(${PROJECT_NAME}_noise_qualification_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_noise_qualification_2d ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools) +#Executable for testing camera noise +add_executable(${PROJECT_NAME}_kinematic_calibration src/tools/kinematic_calibration.cpp) + +set_target_properties(${PROJECT_NAME}_kinematic_calibration PROPERTIES OUTPUT_NAME kinematic_calibration PREFIX "") + +add_dependencies(${PROJECT_NAME}_kinematic_calibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME}_kinematic_calibration ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools) + ############# ## Testing ## ############# diff --git a/rct_examples/src/tools/kinematic_calibration.cpp b/rct_examples/src/tools/kinematic_calibration.cpp new file mode 100644 index 00000000..cfcf4561 --- /dev/null +++ b/rct_examples/src/tools/kinematic_calibration.cpp @@ -0,0 +1,307 @@ +#include +#include + +#include +#include +#include +#include + +#include + +using namespace rct_optimizations; +using namespace rct_ros_tools; + +KinObservation2D3D::Set loadMeasurements(const std::string& filename) +{ + KinObservation2D3D::Set measurements; + + try + { + YAML::Node n = YAML::LoadFile(filename); + measurements.reserve(n.size()); + + for (auto it = n.begin(); it != n.end(); ++it) + { + KinObservation2D3D measurement; + + // Target chain joints + { + YAML::Node joints = it->second["camera_joints"]; + measurement.target_chain_joints.resize(joints.size()); + for (std::size_t i = 0; i < joints.size(); ++i) + { + measurement.target_chain_joints[i] = joints[i].as(); + } + } + + // Camera chain joints + { + YAML::Node joints = it->second["target_joints"]; + measurement.target_chain_joints.resize(joints.size()); + for (std::size_t i = 0; i < joints.size(); ++i) + { + measurement.target_chain_joints[i] = joints[i].as(); + } + } + + // Camera to target pose + YAML::Node pose = it->second["pose"]; + double x = pose["x"].as(); + double y = pose["y"].as(); + double z = pose["z"].as(); + double rx = pose["rx"].as(); + double ry = pose["ry"].as(); + double rz = pose["rz"].as(); + +// measurement.camera_to_target = Eigen::Isometry3d::Identity(); +// measurement.camera_to_target.translate(Eigen::Vector3d(x, y, z)); + + // Euler XYZ +// measurement.camera_to_target.rotate(Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX())); +// measurement.camera_to_target.rotate(Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY())); +// measurement.camera_to_target.rotate(Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ())); + + // Add the measurement to the set + measurements.push_back(measurement); + } + } + catch (YAML::Exception& ex) + { + throw rct_ros_tools::BadFileException(std::string("YAML failure: ") + ex.what()); + } + + return measurements; +} + +std::pair divide(const KinObservation2D3D::Set& measurements, + const double training_pct) +{ + std::vector cal_indices(measurements.size()); + std::iota(cal_indices.begin(), cal_indices.end(), 0); + std::mt19937 mt_rand(std::random_device{}()); + std::shuffle(cal_indices.begin(), cal_indices.end(), mt_rand); + + std::size_t n = static_cast(static_cast(measurements.size()) * training_pct); + + KinObservation2D3D::Set training_set; + { + std::vector training_indices(cal_indices.begin(), cal_indices.begin() + n); + training_set.reserve(training_indices.size()); + for (const std::size_t idx : training_indices) + { + training_set.push_back(measurements.at(idx)); + } + } + + KinObservation2D3D::Set test_set; + { + std::vector test_indices(cal_indices.begin() + n, cal_indices.end()); + test_set.reserve(test_indices.size()); + for (const std::size_t idx : test_indices) + { + test_set.push_back(measurements.at(idx)); + } + } + + return std::make_pair(training_set, test_set); +} + +DHChain createTwoAxisPositioner() +{ + std::vector transforms; + transforms.reserve(2); + + Eigen::Vector4d p1, p2; + p1 << 0.0, 0.0, 0.0, -M_PI / 2.0; + p2 << -0.475, -M_PI / 2.0, 0.0, 0.0; + + // Add the first DH transform + { + DHTransform t(p1, DHJointType::REVOLUTE, "j1"); + t.max = M_PI; + t.min = -M_PI; + transforms.push_back(t); + } + // Add the second DH transform + { + DHTransform dh_transform(p2, DHJointType::REVOLUTE, "j2"); + dh_transform.max = 2.0 * M_PI; + dh_transform.min = -2.0 * M_PI; + transforms.push_back(dh_transform); + } + + // Set an arbitrary base offset + Eigen::Isometry3d base_offset(Eigen::Isometry3d::Identity()); + base_offset.translate(Eigen::Vector3d(2.2, 0.0, 1.6)); + base_offset.rotate(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX())); + + return DHChain(transforms, base_offset); +} + +void test(const DHChain& initial_camera_chain, const DHChain& initial_target_chain, + const KinematicCalibrationResult& result, const KinObservation2D3D::Set& measurements) +{ + // Test the result by moving the robot around to a lot of positions and seeing of the results match + DHChain camera_chain(initial_camera_chain, result.camera_chain_dh_offsets); + DHChain target_chain(initial_target_chain, result.target_chain_dh_offsets); + + namespace ba = boost::accumulators; + ba::accumulator_set> pos_acc; + ba::accumulator_set> ori_acc; + + for (const KinematicMeasurement& m : measurements) + { + // Build the transforms from the camera chain base out to the camera + Eigen::Isometry3d camera_chain_fk = camera_chain.getFK(m.camera_chain_joints); + Eigen::Isometry3d camera_base_to_camera = camera_chain_fk * result.camera_mount_to_camera; + + // Build the transforms from the camera chain base out to the target + Eigen::Isometry3d target_chain_fk = target_chain.getFK(m.target_chain_joints); + Eigen::Isometry3d camera_base_to_target = + result.camera_base_to_target_base * target_chain_fk * result.target_mount_to_target; + + // Now that we have two transforms in the same frame, get the target point in the camera frame + Eigen::Isometry3d camera_to_target = camera_base_to_camera.inverse() * camera_base_to_target; + + // Compare + Eigen::Isometry3d diff = camera_to_target.inverse() * m.camera_to_target; + pos_acc(diff.translation().norm()); + ori_acc(Eigen::Quaterniond(camera_to_target.linear()).angularDistance(Eigen::Quaterniond(m.camera_to_target.linear()))); + } + + std::cout << "Position Difference Mean: " << ba::mean(pos_acc) << std::endl; + std::cout << "Position Difference Std. Dev.: " << std::sqrt(ba::variance(pos_acc)) << std::endl; + + std::cout << "Orientation Difference Mean: " << ba::mean(ori_acc) << std::endl; + std::cout << "Orientation difference Std. Dev.: " << std::sqrt(ba::variance(ori_acc)) << std::endl; +} + +void test2(const DHChain& initial_chain, const KinematicCalibrationResult& result, const KinematicMeasurement::Set& measurements) +{ + // Test the result by moving the robot around to a lot of positions and seeing of the results match + DHChain chain(initial_chain, result.target_chain_dh_offsets); + + namespace ba = boost::accumulators; + ba::accumulator_set> pos_acc; + ba::accumulator_set> ori_acc; + + for (const KinematicMeasurement& m : measurements) + { + // Build the transforms from the camera chain base out to the camera + Eigen::Isometry3d chain_fk = chain.getFK(m.target_chain_joints); +// Eigen::Isometry3d camera_base_to_camera = camera_chain_fk * result.camera_mount_to_camera; + + // Build the transforms from the camera chain base out to the target +// Eigen::Isometry3d camera_base_to_target = +// result.camera_base_to_target_base * target_chain_fk * result.target_mount_to_target; + + // Now that we have two transforms in the same frame, get the target point in the camera frame + Eigen::Isometry3d camera_to_target = result.camera_mount_to_camera.inverse() * chain_fk * result.target_mount_to_target; + + // Compare + Eigen::Isometry3d diff = camera_to_target.inverse() * m.camera_to_target; + pos_acc(diff.translation().norm()); + ori_acc(Eigen::Quaterniond(camera_to_target.linear()).angularDistance(Eigen::Quaterniond(m.camera_to_target.linear()))); + } + + std::cout << "Position Difference Mean: " << ba::mean(pos_acc) << std::endl; + std::cout << "Position Difference Std. Dev.: " << std::sqrt(ba::variance(pos_acc)) << std::endl; + + std::cout << "Orientation Difference Mean: " << ba::mean(ori_acc) << std::endl; + std::cout << "Orientation difference Std. Dev.: " << std::sqrt(ba::variance(ori_acc)) << std::endl; +} + +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, "kinematic_calibration"); + // ros::NodeHandle pnh("~"); + + // std::string data_file; + // if (!get(pnh, "data_file", data_file)) + // return -1; + + + // Load the observations + KinObservation2D3D::Set measurements = loadMeasurements(argv[1]); + + // Split the observations into a training and validation group + std::pair measurement_sets = divide(measurements, 0.8); + + KinematicCalibrationProblem2D3D problem(DHChain({}), createTwoAxisPositioner()); + problem.observations = measurement_sets.first; + problem.target_mount_to_target_guess = Eigen::Isometry3d::Identity(); + problem.target_mount_to_target_guess.translate(Eigen::Vector3d(0.17, -0.65, 0.5)); + problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX())); + problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitY())); + problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(0.2, Eigen::Vector3d::UnitZ())); + + problem.camera_mount_to_camera_guess = Eigen::Isometry3d::Identity(); + problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.2, 0.7, 1.075)); + +// problem.camera_base_to_target_base_guess = Eigen::Isometry3d::Identity(); + + // problem.camera_chain_offset_stdev = 0.001; + // problem.target_chain_offset_stdev = 10.01; + + problem.chain_offset_stdev = 0.1; + + // Mask a few DH parameters in the target chain (index 1) + { + Eigen::Matrix mask = + Eigen::Matrix::Constant(problem.chain.dof(), 4, false); + + // Mask the last row because they duplicate the target mount to target transform + mask.bottomRows(1) << true, true, true, true; + + // Add the mask to the problem + problem.mask.at(0) = createDHMask(mask); + } + + /* Mask the camera base to target base transform (duplicated by target mount to target transform when + * the target chain has no joints */ +// problem.mask.at(6) = { 0, 1, 2 }; +// problem.mask.at(7) = { 0, 1, 2 }; + + // Mask the z-value of the camera mount to camera position +// problem.mask.at(2) = { 0, 1, 2 }; + + KinematicCalibrationResult result = optimize(problem); + + Eigen::IOFormat fmt(4, 0, "|", "\n", "|", "|"); + + std::stringstream ss; + ss << "\nCalibration " << (result.converged ? "did" : "did not") << " converge\n"; + ss << "Initial cost per observation: " << std::sqrt(result.initial_cost_per_obs) << "\n"; + ss << "Final cost per observation: " << std::sqrt(result.final_cost_per_obs) << "\n"; + ss << "\nCamera mount to camera\n" << result.camera_mount_to_camera.matrix().format(fmt) << "\n"; + + ss << result.camera_mount_to_camera.rotation().eulerAngles(0, 1, 2) << "\n"; + + + ss << "\nTarget mount to target\n" << result.target_mount_to_target.matrix().format(fmt) << "\n"; + + ss << result.target_mount_to_target.rotation().eulerAngles(0, 1, 2) << "\n"; + + + ss << "\nTarget chain DH parameter offsets\n" << result.target_chain_dh_offsets.matrix().format(fmt) << "\n"; + ss << "\nCamera chain DH parameter offsets\n" << result.camera_chain_dh_offsets.matrix().format(fmt) << "\n"; + ss << result.covariance.printCorrelationCoeffAboveThreshold(0.5); + + std::cout << ss.str() << std::endl; + + // test(problem.camera_chain, problem.target_chain, result, measurement_sets.second); + test2(problem.chain, result, measurement_sets.second); + + return 0; +} From 8eb2d8e81588dc68f4408d9cec8d7c9b58160846 Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 11 Aug 2020 09:20:43 -0500 Subject: [PATCH 2/7] Added new constructor and method to DHChain class --- .../include/rct_optimizations/dh_chain.h | 8 ++++++++ .../src/rct_optimizations/dh_chain.cpp | 16 ++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/rct_optimizations/include/rct_optimizations/dh_chain.h b/rct_optimizations/include/rct_optimizations/dh_chain.h index 8f6608be..fdf35bb1 100644 --- a/rct_optimizations/include/rct_optimizations/dh_chain.h +++ b/rct_optimizations/include/rct_optimizations/dh_chain.h @@ -119,6 +119,8 @@ class DHChain DHChain(std::vector transforms, const Eigen::Isometry3d& base_offset = Eigen::Isometry3d::Identity()); + DHChain(const DHChain& rhs, const Eigen::MatrixX4d& dh_offsets); + /** * @brief Calculates forward kinematics for the chain with the joints provided. * Note: the transform to the n-th link is calculated, where n is the size of @ref joint_values @@ -199,6 +201,12 @@ class DHChain */ std::vector> getParamLabels() const; + /** + * @brief Gets the base offset of the transform + * @return + */ + Eigen::Isometry3d getBaseOffset() const; + protected: /** @brief The DH transforms that make up the chain */ std::vector transforms_; diff --git a/rct_optimizations/src/rct_optimizations/dh_chain.cpp b/rct_optimizations/src/rct_optimizations/dh_chain.cpp index f1a75c98..ec7f8cd8 100644 --- a/rct_optimizations/src/rct_optimizations/dh_chain.cpp +++ b/rct_optimizations/src/rct_optimizations/dh_chain.cpp @@ -47,6 +47,18 @@ DHChain::DHChain(std::vector transforms, { } +DHChain::DHChain(const DHChain& other, const Eigen::MatrixX4d& dh_offsets) +{ + transforms_.reserve(other.transforms_.size()); + for (std::size_t i = 0; i < other.transforms_.size(); ++i) + { + const DHTransform& transform = other.transforms_.at(i); + transforms_.emplace_back(transform.params + dh_offsets.row(i).transpose(), transform.type); + } + + base_offset_ = other.base_offset_; +} + Eigen::VectorXd DHChain::createUniformlyRandomPose() const { Eigen::VectorXd joints(transforms_.size()); @@ -93,5 +105,9 @@ std::vector> DHChain::getParamLabels() const return out; } +Eigen::Isometry3d DHChain::getBaseOffset() const +{ + return base_offset_; +} } // namespace rct_optimizations From 316cfb329a52bee06cc8b694740e02c0ffee8781 Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 18 Aug 2020 17:36:26 -0500 Subject: [PATCH 3/7] Add parameters for standard deviation expectation --- .../rct_optimizations/dh_chain_kinematic_calibration.h | 3 +++ .../rct_optimizations/dh_chain_kinematic_calibration.cpp | 6 +++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h index 884ee77a..71539fad 100644 --- a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h +++ b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h @@ -66,6 +66,9 @@ struct KinematicCalibrationProblem2D3D */ std::array, 8> mask; + double camera_chain_dh_stdev_expectation; + double target_chain_dh_stdev_expectation; + std::string label_camera_mount_to_camera = "camera_mount_to_camera"; std::string label_target_mount_to_target = "target_mount_to_target"; std::string label_camera_base_to_target = "camera_base_to_target"; diff --git a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp index 6d97a6e9..e8da9291 100644 --- a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp +++ b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp @@ -175,7 +175,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m Eigen::ArrayXXd stdev(Eigen::ArrayXXd::Constant(camera_chain_dh_offsets.rows(), camera_chain_dh_offsets.cols(), - 1.0e-3)); + params.target_chain_dh_stdev_expectation)); auto *fn = new MaximumLikelihood(mean, stdev); auto *cost_block = new ceres::DynamicAutoDiffCostFunction(fn); @@ -192,8 +192,8 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m Eigen::ArrayXXd::Zero(target_chain_dh_offsets.rows(), target_chain_dh_offsets.cols())); Eigen::ArrayXXd stdev(Eigen::ArrayXXd::Constant(target_chain_dh_offsets.rows(), - target_chain_dh_offsets.cols(), - 1.0e-3)); + target_chain_dh_offsets.cols(), + params.target_chain_dh_stdev_expectation)); auto *fn = new MaximumLikelihood(mean, stdev); auto *cost_block = new ceres::DynamicAutoDiffCostFunction(fn); From 7c839b5ba1ab916904459ed6352a03767440af1b Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 18 Aug 2020 17:36:49 -0500 Subject: [PATCH 4/7] WIP updates to calibration routine --- .../src/tools/kinematic_calibration.cpp | 304 +++++++++++------- 1 file changed, 192 insertions(+), 112 deletions(-) diff --git a/rct_examples/src/tools/kinematic_calibration.cpp b/rct_examples/src/tools/kinematic_calibration.cpp index cfcf4561..51e0c74c 100644 --- a/rct_examples/src/tools/kinematic_calibration.cpp +++ b/rct_examples/src/tools/kinematic_calibration.cpp @@ -1,73 +1,144 @@ #include +#include +#include #include +#include #include #include -#include -#include - +#include #include +#include +#include using namespace rct_optimizations; using namespace rct_ros_tools; +using namespace rct_image_tools; + +CircleDetectorParams createDetectorParams() +{ + CircleDetectorParams params; + params.minThreshold = 50; + params.maxThreshold = 220; + params.nThresholds = 20; + + params.minRepeatability = 1; + params.circleInclusionRadius = 10; + params.maxRadiusDiff = 10; + + params.maxAverageEllipseError = 0.002; + + params.filterByArea = true; + params.minArea = 200.0; + params.maxArea = 10000.0; -KinObservation2D3D::Set loadMeasurements(const std::string& filename) + params.filterByConvexity = false; + params.minConvexity = 0.95f; + params.maxConvexity = 1.05f; + + params.filterByColor = false; + params.circleColor = 0; + + params.filterByInertia = false; + params.filterByCircularity = false; + + return params; +} + +KinObservation2D3D::Set loadMeasurements(const std::string& filename, const ModifiedCircleGridObservationFinder& target_finder) { KinObservation2D3D::Set measurements; + std::vector target_points = target_finder.target().createPoints(); - try - { - YAML::Node n = YAML::LoadFile(filename); - measurements.reserve(n.size()); + // Load the data YAML file + YAML::Node n = YAML::LoadFile(filename); + measurements.reserve(n.size()); - for (auto it = n.begin(); it != n.end(); ++it) + // Loop over all observations + for (auto it = n.begin(); it != n.end(); ++it) + { + KinObservation2D3D observation; + + // Load the image + std::string image_file = "ros/" + it->first.as() + "_image.png"; + cv::Mat image = cv::Scalar::all(255) - cv::imread(image_file, CV_LOAD_IMAGE_COLOR); // TODO: Is CV_LOAD_IMAGE_COLOR needed? + if (image.data == NULL) + throw std::runtime_error("File failed to load or does not exist: " + image_file); + + // Find the target + CircleDetectorParams detector_params = createDetectorParams(); + CircleDetector detector(detector_params); + cv::Mat debug_image = detector.drawDetectedCircles(image); + cv::namedWindow("circle_detection_debug", 0); + cv::resizeWindow("circle_detection_debug", 1000, 800); + cv::imshow("circle_detection_debug", debug_image); + cv::waitKey(); + + auto features = target_finder.findObservations(image, &detector_params); + if (!features) { - KinObservation2D3D measurement; + ROS_WARN_STREAM("Failed to find observations for image '" << image_file << "'"); + cv::imshow("circle_detection_debug", image); + cv::waitKey(); + continue; + } + else + { + cv::Mat obs_image = target_finder.drawObservations(image, features.get()); + cv::imshow("circle_detection_debug", obs_image); - // Target chain joints - { - YAML::Node joints = it->second["camera_joints"]; - measurement.target_chain_joints.resize(joints.size()); - for (std::size_t i = 0; i < joints.size(); ++i) + bool accepted = false; + cv::MouseCallback cb = [](int event, int x, int y, int flags, void* userdata) { + if(event == cv::EVENT_FLAG_LBUTTON) { - measurement.target_chain_joints[i] = joints[i].as(); + bool* val = reinterpret_cast(userdata); + *val = true; } + }; + + cv::setMouseCallback("circle_detection_debug", cb, &accepted); + + cv::waitKey(); + + if (!accepted) + { + ROS_WARN_STREAM("Not accepted!"); + continue; } + } - // Camera chain joints + // Create the correspondences + observation.correspondence_set.reserve(features->size()); + for (std::size_t i = 0; i < target_points.size(); ++i) + { + Correspondence2D3D corr; + corr.in_image = features->at(i); + corr.in_target = target_points.at(i); + observation.correspondence_set.push_back(corr); + } + + // Target chain joints + { + YAML::Node joints = it->second["camera_joints"]; + observation.camera_chain_joints.resize(joints.size()); + for (std::size_t i = 0; i < joints.size(); ++i) { - YAML::Node joints = it->second["target_joints"]; - measurement.target_chain_joints.resize(joints.size()); - for (std::size_t i = 0; i < joints.size(); ++i) - { - measurement.target_chain_joints[i] = joints[i].as(); - } + observation.camera_chain_joints[i] = joints[i].as(); } + } - // Camera to target pose - YAML::Node pose = it->second["pose"]; - double x = pose["x"].as(); - double y = pose["y"].as(); - double z = pose["z"].as(); - double rx = pose["rx"].as(); - double ry = pose["ry"].as(); - double rz = pose["rz"].as(); - -// measurement.camera_to_target = Eigen::Isometry3d::Identity(); -// measurement.camera_to_target.translate(Eigen::Vector3d(x, y, z)); - - // Euler XYZ -// measurement.camera_to_target.rotate(Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX())); -// measurement.camera_to_target.rotate(Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY())); -// measurement.camera_to_target.rotate(Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ())); - - // Add the measurement to the set - measurements.push_back(measurement); + // Camera chain joints + { + YAML::Node joints = it->second["target_joints"]; + observation.target_chain_joints.resize(joints.size()); + for (std::size_t i = 0; i < joints.size(); ++i) + { + observation.target_chain_joints[i] = joints[i].as(); + } } - } - catch (YAML::Exception& ex) - { - throw rct_ros_tools::BadFileException(std::string("YAML failure: ") + ex.what()); + + // Add the observation to the set + measurements.push_back(observation); } return measurements; @@ -139,7 +210,8 @@ DHChain createTwoAxisPositioner() } void test(const DHChain& initial_camera_chain, const DHChain& initial_target_chain, - const KinematicCalibrationResult& result, const KinObservation2D3D::Set& measurements) + const KinematicCalibrationResult& result, const KinObservation2D3D::Set& observations, + const CameraIntrinsics& intr) { // Test the result by moving the robot around to a lot of positions and seeing of the results match DHChain camera_chain(initial_camera_chain, result.camera_chain_dh_offsets); @@ -149,59 +221,37 @@ void test(const DHChain& initial_camera_chain, const DHChain& initial_target_cha ba::accumulator_set> pos_acc; ba::accumulator_set> ori_acc; - for (const KinematicMeasurement& m : measurements) + for (const KinObservation2D3D& obs : observations) { // Build the transforms from the camera chain base out to the camera - Eigen::Isometry3d camera_chain_fk = camera_chain.getFK(m.camera_chain_joints); + Eigen::Isometry3d camera_chain_fk = camera_chain.getFK(obs.camera_chain_joints); Eigen::Isometry3d camera_base_to_camera = camera_chain_fk * result.camera_mount_to_camera; // Build the transforms from the camera chain base out to the target - Eigen::Isometry3d target_chain_fk = target_chain.getFK(m.target_chain_joints); + Eigen::Isometry3d target_chain_fk = target_chain.getFK(obs.target_chain_joints); Eigen::Isometry3d camera_base_to_target = result.camera_base_to_target_base * target_chain_fk * result.target_mount_to_target; // Now that we have two transforms in the same frame, get the target point in the camera frame Eigen::Isometry3d camera_to_target = camera_base_to_camera.inverse() * camera_base_to_target; - // Compare - Eigen::Isometry3d diff = camera_to_target.inverse() * m.camera_to_target; - pos_acc(diff.translation().norm()); - ori_acc(Eigen::Quaterniond(camera_to_target.linear()).angularDistance(Eigen::Quaterniond(m.camera_to_target.linear()))); - } - - std::cout << "Position Difference Mean: " << ba::mean(pos_acc) << std::endl; - std::cout << "Position Difference Std. Dev.: " << std::sqrt(ba::variance(pos_acc)) << std::endl; - - std::cout << "Orientation Difference Mean: " << ba::mean(ori_acc) << std::endl; - std::cout << "Orientation difference Std. Dev.: " << std::sqrt(ba::variance(ori_acc)) << std::endl; -} - -void test2(const DHChain& initial_chain, const KinematicCalibrationResult& result, const KinematicMeasurement::Set& measurements) -{ - // Test the result by moving the robot around to a lot of positions and seeing of the results match - DHChain chain(initial_chain, result.target_chain_dh_offsets); - - namespace ba = boost::accumulators; - ba::accumulator_set> pos_acc; - ba::accumulator_set> ori_acc; - - for (const KinematicMeasurement& m : measurements) - { - // Build the transforms from the camera chain base out to the camera - Eigen::Isometry3d chain_fk = chain.getFK(m.target_chain_joints); -// Eigen::Isometry3d camera_base_to_camera = camera_chain_fk * result.camera_mount_to_camera; + PnPProblem pnp; + pnp.camera_to_target_guess = camera_to_target; + pnp.correspondences = obs.correspondence_set; + pnp.intr = intr; + PnPResult result = optimize(pnp); - // Build the transforms from the camera chain base out to the target -// Eigen::Isometry3d camera_base_to_target = -// result.camera_base_to_target_base * target_chain_fk * result.target_mount_to_target; - - // Now that we have two transforms in the same frame, get the target point in the camera frame - Eigen::Isometry3d camera_to_target = result.camera_mount_to_camera.inverse() * chain_fk * result.target_mount_to_target; - - // Compare - Eigen::Isometry3d diff = camera_to_target.inverse() * m.camera_to_target; - pos_acc(diff.translation().norm()); - ori_acc(Eigen::Quaterniond(camera_to_target.linear()).angularDistance(Eigen::Quaterniond(m.camera_to_target.linear()))); + if(result.converged) + { + // Compare + Eigen::Isometry3d diff = camera_to_target.inverse() * result.camera_to_target; + pos_acc(diff.translation().norm()); + ori_acc(Eigen::Quaterniond(camera_to_target.linear()).angularDistance(Eigen::Quaterniond(result.camera_to_target.linear()))); + } + else + { + ROS_WARN_STREAM("PnP optimization failed"); + } } std::cout << "Position Difference Mean: " << ba::mean(pos_acc) << std::endl; @@ -231,47 +281,80 @@ int main(int argc, char** argv) // if (!get(pnh, "data_file", data_file)) // return -1; + ModifiedCircleGridTarget target(9, 7, 0.078581); +// ModifiedCircleGridTarget target(7, 5, 0.06); + ModifiedCircleGridObservationFinder target_finder(target); // Load the observations - KinObservation2D3D::Set measurements = loadMeasurements(argv[1]); + KinObservation2D3D::Set measurements = loadMeasurements(argv[1], target_finder); // Split the observations into a training and validation group - std::pair measurement_sets = divide(measurements, 0.8); + std::pair measurement_sets = divide(measurements, 0.8); KinematicCalibrationProblem2D3D problem(DHChain({}), createTwoAxisPositioner()); + problem.intr.fx() = 2246.59; + problem.intr.fy() = 2245.66; + problem.intr.cx() = 1039.16; + problem.intr.cy() = 800.869; + problem.observations = measurement_sets.first; + ROS_INFO_STREAM("Performing calibration with " << problem.observations.size() << " observations"); + problem.target_mount_to_target_guess = Eigen::Isometry3d::Identity(); - problem.target_mount_to_target_guess.translate(Eigen::Vector3d(0.17, -0.65, 0.5)); - problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX())); - problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitY())); - problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(0.2, Eigen::Vector3d::UnitZ())); - problem.camera_mount_to_camera_guess = Eigen::Isometry3d::Identity(); - problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.2, 0.7, 1.075)); +// // Vicon +// { +// problem.target_mount_to_target_guess.translate(Eigen::Vector3d(0.17, -0.65, 0.5)); +// problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX())); +// problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitY())); +// problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(0.2, Eigen::Vector3d::UnitZ())); -// problem.camera_base_to_target_base_guess = Eigen::Isometry3d::Identity(); +// problem.camera_mount_to_camera_guess = Eigen::Isometry3d::Identity(); +// problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.2, 0.7, 1.075)); +// } - // problem.camera_chain_offset_stdev = 0.001; - // problem.target_chain_offset_stdev = 10.01; +// // 7x5 Target +// { +// problem.target_mount_to_target_guess.translate(Eigen::Vector3d(-0.2, -0.15, 0.7)); +// problem.target_mount_to_target_guess.rotate(Eigen::Quaterniond(0.743, 0.0, 0.0, -0.669)); - problem.chain_offset_stdev = 0.1; +// problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.58, -1.6, 2.83)); +// problem.camera_mount_to_camera_guess.rotate(Eigen::Quaterniond(-0.463, 0.883, 0.002, -0.075)); +// } + + // 9x7 Target + { + problem.target_mount_to_target_guess.translate(Eigen::Vector3d(0.2806, -0.5019, 0.7042)); +// problem.target_mount_to_target_guess.rotate(Eigen::Quaterniond(0.669, 0.0, 0.0, 0.743)); + problem.target_mount_to_target_guess.rotate(Eigen::Quaterniond(0.6744918, -0.0005498, -0.0008313, 0.7382817)); + + problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.555, -1.762, 2.832)); +// problem.camera_mount_to_camera_guess.rotate(Eigen::Quaterniond(-0.463, 0.883, 0.002, -0.075)); + problem.camera_mount_to_camera_guess.rotate(Eigen::Quaterniond(0.4608509, -0.8808954, -0.0562409, 0.092069)); + } + + + problem.camera_base_to_target_base_guess = Eigen::Isometry3d::Identity(); + + problem.camera_chain_dh_stdev_expectation = 0.001; + problem.target_chain_dh_stdev_expectation = 0.005; // Mask a few DH parameters in the target chain (index 1) { Eigen::Matrix mask = - Eigen::Matrix::Constant(problem.chain.dof(), 4, false); + Eigen::Matrix::Constant(problem.target_chain.dof(), 4, false); // Mask the last row because they duplicate the target mount to target transform mask.bottomRows(1) << true, true, true, true; // Add the mask to the problem - problem.mask.at(0) = createDHMask(mask); + problem.mask.at(1) = createDHMask(mask); } /* Mask the camera base to target base transform (duplicated by target mount to target transform when * the target chain has no joints */ -// problem.mask.at(6) = { 0, 1, 2 }; -// problem.mask.at(7) = { 0, 1, 2 }; + problem.mask.at(6) = { 0, 1, 2 }; + problem.mask.at(7) = { 0, 1, 2 }; // Mask the z-value of the camera mount to camera position // problem.mask.at(2) = { 0, 1, 2 }; @@ -284,15 +367,12 @@ int main(int argc, char** argv) ss << "\nCalibration " << (result.converged ? "did" : "did not") << " converge\n"; ss << "Initial cost per observation: " << std::sqrt(result.initial_cost_per_obs) << "\n"; ss << "Final cost per observation: " << std::sqrt(result.final_cost_per_obs) << "\n"; - ss << "\nCamera mount to camera\n" << result.camera_mount_to_camera.matrix().format(fmt) << "\n"; - - ss << result.camera_mount_to_camera.rotation().eulerAngles(0, 1, 2) << "\n"; + ss << "\nCamera mount to camera\n" << result.camera_mount_to_camera.matrix().format(fmt) << "\n"; + ss << result.camera_mount_to_camera.rotation().eulerAngles(0, 1, 2).transpose() << "\n"; ss << "\nTarget mount to target\n" << result.target_mount_to_target.matrix().format(fmt) << "\n"; - - ss << result.target_mount_to_target.rotation().eulerAngles(0, 1, 2) << "\n"; - + ss << result.target_mount_to_target.rotation().eulerAngles(0, 1, 2).transpose() << "\n"; ss << "\nTarget chain DH parameter offsets\n" << result.target_chain_dh_offsets.matrix().format(fmt) << "\n"; ss << "\nCamera chain DH parameter offsets\n" << result.camera_chain_dh_offsets.matrix().format(fmt) << "\n"; @@ -300,8 +380,8 @@ int main(int argc, char** argv) std::cout << ss.str() << std::endl; - // test(problem.camera_chain, problem.target_chain, result, measurement_sets.second); - test2(problem.chain, result, measurement_sets.second); + ROS_INFO_STREAM("Validating calibration with " << measurement_sets.second.size() << " observations"); + test(problem.camera_chain, problem.target_chain, result, measurement_sets.second, problem.intr); return 0; } From d0a005d5f0199ad3f5c78527de7f1561bed914e4 Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 25 Aug 2020 17:03:44 -0500 Subject: [PATCH 5/7] Parameterized solver options; added checks to additions of expectation costs --- .../dh_chain_kinematic_calibration.h | 3 ++- .../dh_chain_kinematic_calibration.cpp | 15 +++++---------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h index 71539fad..3be9ef22 100644 --- a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h +++ b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h @@ -240,7 +240,8 @@ class DualDHChainCost2D3D Eigen::VectorXd target_chain_joints_; }; -KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &problem); +KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &problem, + const ceres::Solver::Options& options = ceres::Solver::Options()); } // namespace rct_optimizations diff --git a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp index e8da9291..485d604c 100644 --- a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp +++ b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp @@ -23,7 +23,8 @@ Eigen::Isometry3d createTransform(const Eigen::Vector3d& t, const Eigen::Vector3 return result; } -KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶ms) +KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶ms, + const ceres::Solver::Options& options) { // Initialize the optimization variables // Camera mount to camera (cm_to_c) unnormalized angle axis and translation @@ -168,7 +169,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m addSubsetParameterization(problem, params.mask, tmp); // Add a cost to drive the camera chain DH parameters towards an expected mean - if (params.camera_chain.dof() != 0) + if (params.camera_chain.dof() != 0 && !problem.IsParameterBlockConstant(parameters[0])) { Eigen::ArrayXXd mean( Eigen::ArrayXXd::Zero(camera_chain_dh_offsets.rows(), camera_chain_dh_offsets.cols())); @@ -186,7 +187,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m } // Add a cost to drive the target chain DH parameters towards an expected mean - if (params.target_chain.dof() != 0) + if (params.target_chain.dof() != 0 && !problem.IsParameterBlockConstant(parameters[1])) { Eigen::ArrayXXd mean( Eigen::ArrayXXd::Zero(target_chain_dh_offsets.rows(), target_chain_dh_offsets.cols())); @@ -203,14 +204,8 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m problem.AddResidualBlock(cost_block, nullptr, target_chain_dh_offsets.data()); } - // Setup the Ceres optimization parameters - ceres::Solver::Options options; - options.max_num_iterations = 150; - options.num_threads = 4; - options.minimizer_progress_to_stdout = true; - ceres::Solver::Summary summary; - // Solve the optimization + ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); // Report and save the results From 2866e31ab079034b49e8d31c5ca7bcee63c2c513 Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 25 Aug 2020 17:06:31 -0500 Subject: [PATCH 6/7] Updated parameterization of example --- .../src/tools/kinematic_calibration.cpp | 129 +++++------------- 1 file changed, 32 insertions(+), 97 deletions(-) diff --git a/rct_examples/src/tools/kinematic_calibration.cpp b/rct_examples/src/tools/kinematic_calibration.cpp index 51e0c74c..5244a2b5 100644 --- a/rct_examples/src/tools/kinematic_calibration.cpp +++ b/rct_examples/src/tools/kinematic_calibration.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -77,7 +78,7 @@ KinObservation2D3D::Set loadMeasurements(const std::string& filename, const Modi auto features = target_finder.findObservations(image, &detector_params); if (!features) { - ROS_WARN_STREAM("Failed to find observations for image '" << image_file << "'"); + std::cout << "Failed to find observations for image '" << image_file << "'" << std::endl; cv::imshow("circle_detection_debug", image); cv::waitKey(); continue; @@ -97,14 +98,12 @@ KinObservation2D3D::Set loadMeasurements(const std::string& filename, const Modi }; cv::setMouseCallback("circle_detection_debug", cb, &accepted); - cv::waitKey(); if (!accepted) - { - ROS_WARN_STREAM("Not accepted!"); continue; - } + + std::cout << "Image accepted!" << std::endl; } // Create the correspondences @@ -144,39 +143,6 @@ KinObservation2D3D::Set loadMeasurements(const std::string& filename, const Modi return measurements; } -std::pair divide(const KinObservation2D3D::Set& measurements, - const double training_pct) -{ - std::vector cal_indices(measurements.size()); - std::iota(cal_indices.begin(), cal_indices.end(), 0); - std::mt19937 mt_rand(std::random_device{}()); - std::shuffle(cal_indices.begin(), cal_indices.end(), mt_rand); - - std::size_t n = static_cast(static_cast(measurements.size()) * training_pct); - - KinObservation2D3D::Set training_set; - { - std::vector training_indices(cal_indices.begin(), cal_indices.begin() + n); - training_set.reserve(training_indices.size()); - for (const std::size_t idx : training_indices) - { - training_set.push_back(measurements.at(idx)); - } - } - - KinObservation2D3D::Set test_set; - { - std::vector test_indices(cal_indices.begin() + n, cal_indices.end()); - test_set.reserve(test_indices.size()); - for (const std::size_t idx : test_indices) - { - test_set.push_back(measurements.at(idx)); - } - } - - return std::make_pair(training_set, test_set); -} - DHChain createTwoAxisPositioner() { std::vector transforms; @@ -250,7 +216,7 @@ void test(const DHChain& initial_camera_chain, const DHChain& initial_target_cha } else { - ROS_WARN_STREAM("PnP optimization failed"); + std::cout << "PnP optimization failed" << std::endl; } } @@ -274,67 +240,30 @@ bool get(const ros::NodeHandle& nh, const std::string& key, T& val) int main(int argc, char** argv) { - // ros::init(argc, argv, "kinematic_calibration"); - // ros::NodeHandle pnh("~"); - - // std::string data_file; - // if (!get(pnh, "data_file", data_file)) - // return -1; + if (argc != 6) + { + std::cout << "Incorrect number of arguments: " << argc << std::endl; + return -1; + } - ModifiedCircleGridTarget target(9, 7, 0.078581); -// ModifiedCircleGridTarget target(7, 5, 0.06); + ModifiedCircleGridTarget target = TargetLoader::load(argv[2]); ModifiedCircleGridObservationFinder target_finder(target); - // Load the observations - KinObservation2D3D::Set measurements = loadMeasurements(argv[1], target_finder); - - // Split the observations into a training and validation group - std::pair measurement_sets = divide(measurements, 0.8); - + // Create the problem KinematicCalibrationProblem2D3D problem(DHChain({}), createTwoAxisPositioner()); - problem.intr.fx() = 2246.59; - problem.intr.fy() = 2245.66; - problem.intr.cx() = 1039.16; - problem.intr.cy() = 800.869; - - problem.observations = measurement_sets.first; - ROS_INFO_STREAM("Performing calibration with " << problem.observations.size() << " observations"); - problem.target_mount_to_target_guess = Eigen::Isometry3d::Identity(); + // Load the camera intrinsics + problem.intr = loadIntrinsics(argv[3]); -// // Vicon -// { -// problem.target_mount_to_target_guess.translate(Eigen::Vector3d(0.17, -0.65, 0.5)); -// problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX())); -// problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitY())); -// problem.target_mount_to_target_guess.rotate(Eigen::AngleAxisd(0.2, Eigen::Vector3d::UnitZ())); - -// problem.camera_mount_to_camera_guess = Eigen::Isometry3d::Identity(); -// problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.2, 0.7, 1.075)); -// } - -// // 7x5 Target -// { -// problem.target_mount_to_target_guess.translate(Eigen::Vector3d(-0.2, -0.15, 0.7)); -// problem.target_mount_to_target_guess.rotate(Eigen::Quaterniond(0.743, 0.0, 0.0, -0.669)); - -// problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.58, -1.6, 2.83)); -// problem.camera_mount_to_camera_guess.rotate(Eigen::Quaterniond(-0.463, 0.883, 0.002, -0.075)); -// } - - // 9x7 Target - { - problem.target_mount_to_target_guess.translate(Eigen::Vector3d(0.2806, -0.5019, 0.7042)); -// problem.target_mount_to_target_guess.rotate(Eigen::Quaterniond(0.669, 0.0, 0.0, 0.743)); - problem.target_mount_to_target_guess.rotate(Eigen::Quaterniond(0.6744918, -0.0005498, -0.0008313, 0.7382817)); - - problem.camera_mount_to_camera_guess.translate(Eigen::Vector3d(2.555, -1.762, 2.832)); -// problem.camera_mount_to_camera_guess.rotate(Eigen::Quaterniond(-0.463, 0.883, 0.002, -0.075)); - problem.camera_mount_to_camera_guess.rotate(Eigen::Quaterniond(0.4608509, -0.8808954, -0.0562409, 0.092069)); - } + // Load the pose guesses + problem.target_mount_to_target_guess = loadPose(argv[4]); + problem.camera_mount_to_camera_guess = loadPose(argv[5]); + // Load the observations + KinObservation2D3D::Set measurements = loadMeasurements(argv[1], target_finder); - problem.camera_base_to_target_base_guess = Eigen::Isometry3d::Identity(); + problem.observations = measurements; + std::cout << "Performing calibration with " << problem.observations.size() << " observations" << std::endl; problem.camera_chain_dh_stdev_expectation = 0.001; problem.target_chain_dh_stdev_expectation = 0.005; @@ -356,10 +285,16 @@ int main(int argc, char** argv) problem.mask.at(6) = { 0, 1, 2 }; problem.mask.at(7) = { 0, 1, 2 }; - // Mask the z-value of the camera mount to camera position -// problem.mask.at(2) = { 0, 1, 2 }; + ceres::Solver::Options options; + options.num_threads = 4; + options.minimizer_progress_to_stdout = true; + options.max_num_iterations = 500; + options.use_nonmonotonic_steps = true; + options.function_tolerance = 1.0e-12; + options.parameter_tolerance = 1.0e-20; + options.gradient_tolerance = 1.0e-16; - KinematicCalibrationResult result = optimize(problem); + KinematicCalibrationResult result = optimize(problem, options); Eigen::IOFormat fmt(4, 0, "|", "\n", "|", "|"); @@ -380,8 +315,8 @@ int main(int argc, char** argv) std::cout << ss.str() << std::endl; - ROS_INFO_STREAM("Validating calibration with " << measurement_sets.second.size() << " observations"); - test(problem.camera_chain, problem.target_chain, result, measurement_sets.second, problem.intr); + ROS_INFO_STREAM("Validating calibration with " << measurements.size() << " observations"); + test(problem.camera_chain, problem.target_chain, result, measurements, problem.intr); return 0; } From c1c53060335853e09d4eb7b8dea057946b6b2082 Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 25 Aug 2020 17:23:20 -0500 Subject: [PATCH 7/7] Updated result printing --- .../src/tools/kinematic_calibration.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/rct_examples/src/tools/kinematic_calibration.cpp b/rct_examples/src/tools/kinematic_calibration.cpp index 5244a2b5..50db442f 100644 --- a/rct_examples/src/tools/kinematic_calibration.cpp +++ b/rct_examples/src/tools/kinematic_calibration.cpp @@ -175,14 +175,10 @@ DHChain createTwoAxisPositioner() return DHChain(transforms, base_offset); } -void test(const DHChain& initial_camera_chain, const DHChain& initial_target_chain, +void test(const DHChain& camera_chain, const DHChain& target_chain, const KinematicCalibrationResult& result, const KinObservation2D3D::Set& observations, const CameraIntrinsics& intr) { - // Test the result by moving the robot around to a lot of positions and seeing of the results match - DHChain camera_chain(initial_camera_chain, result.camera_chain_dh_offsets); - DHChain target_chain(initial_target_chain, result.target_chain_dh_offsets); - namespace ba = boost::accumulators; ba::accumulator_set> pos_acc; ba::accumulator_set> ori_acc; @@ -296,7 +292,7 @@ int main(int argc, char** argv) KinematicCalibrationResult result = optimize(problem, options); - Eigen::IOFormat fmt(4, 0, "|", "\n", "|", "|"); + Eigen::IOFormat fmt(6, 0, "|", "\n", "|", "|"); std::stringstream ss; ss << "\nCalibration " << (result.converged ? "did" : "did not") << " converge\n"; @@ -304,10 +300,10 @@ int main(int argc, char** argv) ss << "Final cost per observation: " << std::sqrt(result.final_cost_per_obs) << "\n"; ss << "\nCamera mount to camera\n" << result.camera_mount_to_camera.matrix().format(fmt) << "\n"; - ss << result.camera_mount_to_camera.rotation().eulerAngles(0, 1, 2).transpose() << "\n"; + ss << "Euler ZYX: " << result.camera_mount_to_camera.rotation().eulerAngles(2, 1, 0).transpose().format(fmt) << "\n"; ss << "\nTarget mount to target\n" << result.target_mount_to_target.matrix().format(fmt) << "\n"; - ss << result.target_mount_to_target.rotation().eulerAngles(0, 1, 2).transpose() << "\n"; + ss << "Euler ZYX: " << result.target_mount_to_target.rotation().eulerAngles(2, 1, 0).transpose().format(fmt) << "\n"; ss << "\nTarget chain DH parameter offsets\n" << result.target_chain_dh_offsets.matrix().format(fmt) << "\n"; ss << "\nCamera chain DH parameter offsets\n" << result.camera_chain_dh_offsets.matrix().format(fmt) << "\n"; @@ -315,8 +311,15 @@ int main(int argc, char** argv) std::cout << ss.str() << std::endl; - ROS_INFO_STREAM("Validating calibration with " << measurements.size() << " observations"); - test(problem.camera_chain, problem.target_chain, result, measurements, problem.intr); + // Test the result by moving the robot around to a lot of positions and seeing of the results match + DHChain optimized_camera_chain(problem.camera_chain, result.camera_chain_dh_offsets); + DHChain optimized_target_chain(problem.target_chain, result.target_chain_dh_offsets); + + std::cout << "Optimized camera chain DH parameters\n" << optimized_camera_chain.getDHTable().format(fmt) << std::endl; + std::cout << "Optimized target chain DH parameters\n" << optimized_target_chain.getDHTable().format(fmt) << std::endl; + + std::cout << "\nValidating calibration with " << measurements.size() << " observations" << std::endl; + test(optimized_camera_chain, optimized_target_chain, result, measurements, problem.intr); return 0; }