From 64602e30a9a6739a495d9fa85dd16253f2972893 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 27 Mar 2020 16:20:46 -0500 Subject: [PATCH] Update library to use Isometry3d instead of Affine3d (#31) --- rct_examples/src/examples/camera_on_wrist.cpp | 6 +-- .../src/tools/camera_on_wrist_extrinsic.cpp | 10 ++--- .../tools/multi_static_camera_extrinsic.cpp | 14 +++---- ...lti_static_camera_multi_step_extrinsic.cpp | 18 ++++----- .../src/tools/solve_multi_camera_pnp.cpp | 10 ++--- rct_examples/src/tools/solve_pnp.cpp | 6 +-- .../src/tools/static_camera_extrinsic.cpp | 10 ++--- .../include/rct_image_tools/image_utils.h | 2 +- .../rct_optimizations/eigen_conversions.h | 4 +- .../experimental/camera_intrinsic.h | 4 +- .../experimental/multi_camera_pnp.h | 6 +-- .../rct_optimizations/experimental/pnp.h | 4 +- .../extrinsic_3d_camera_on_wrist.h | 10 ++--- .../extrinsic_camera_on_wrist.h | 10 ++--- .../extrinsic_multi_static_camera.h | 10 ++--- .../extrinsic_multi_static_camera_only.h | 8 ++-- ...extrinsic_multi_static_camera_wrist_only.h | 10 ++--- .../extrinsic_static_camera.h | 10 ++--- .../rct_optimizations/camera_intrinsic.cpp | 2 +- .../rct_optimizations/eigen_conversions.cpp | 6 +-- .../extrinsic_3d_camera_on_wrist.cpp | 2 +- .../extrinsic_camera_on_wrist.cpp | 2 +- .../extrinsic_multi_static_camera.cpp | 2 +- .../extrinsic_multi_static_camera_only.cpp | 2 +- ...trinsic_multi_static_camera_wrist_only.cpp | 8 ++-- .../extrinsic_static_camera.cpp | 2 +- .../rct_optimizations/multi_camera_pnp.cpp | 2 +- rct_optimizations/test/conversion_utest.cpp | 4 +- .../test/extrinsic_camera_on_wrist_utest.cpp | 14 +++---- .../extrinsic_multi_static_camera_utest.cpp | 40 +++++++++---------- .../observation_creator.h | 4 +- .../rct_optimizations_tests/utilities.h | 2 +- .../test/src/observation_creator.cpp | 8 ++-- rct_optimizations/test/src/utilities.cpp | 4 +- .../include/rct_ros_tools/data_set.h | 2 +- .../include/rct_ros_tools/parameter_loaders.h | 4 +- .../include/rct_ros_tools/print_utils.h | 6 +-- rct_ros_tools/src/command_line_cal.cpp | 2 +- rct_ros_tools/src/data_loader/data_set.cpp | 4 +- .../src/data_loader/parameter_loaders.cpp | 8 ++-- 40 files changed, 141 insertions(+), 141 deletions(-) diff --git a/rct_examples/src/examples/camera_on_wrist.cpp b/rct_examples/src/examples/camera_on_wrist.cpp index cca7dcb4..7ea23f90 100644 --- a/rct_examples/src/examples/camera_on_wrist.cpp +++ b/rct_examples/src/examples/camera_on_wrist.cpp @@ -78,7 +78,7 @@ int extrinsicWristCameraCalibration() } // Now that its loaded let's create some aliases to make this nicer const std::vector& image_set = maybe_data_set->images; - const std::vector& wrist_poses = maybe_data_set->tool_poses; + const std::vector& wrist_poses = maybe_data_set->tool_poses; // Step 3.2: We need to conver the images of calibration targets into sets of correspondences. // In our case, each dot on the target becomes a correspondence: A pair of positions, one for @@ -139,11 +139,11 @@ int extrinsicWristCameraCalibration() // Note: Convergence and low cost does not mean a good calibration. See the calibration primer // readme on the main page of this repo. - Eigen::Affine3d c = opt_result.wrist_to_camera; + Eigen::Isometry3d c = opt_result.wrist_to_camera; rct_ros_tools::printTransform(c, "Wrist", "Camera", "WRIST TO CAMERA"); rct_ros_tools::printNewLine(); - Eigen::Affine3d t = opt_result.base_to_target; + Eigen::Isometry3d t = opt_result.base_to_target; rct_ros_tools::printTransform(t, "Base", "Target", "BASE TO TARGET"); rct_ros_tools::printNewLine(); diff --git a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp b/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp index 3d208362..cfc3dc0d 100644 --- a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp +++ b/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp @@ -15,14 +15,14 @@ #include #include -static void reproject(const Eigen::Affine3d& wrist_to_camera, const Eigen::Affine3d& base_to_target, - const Eigen::Affine3d& base_to_wrist, const rct_optimizations::CameraIntrinsics& intr, +static void reproject(const Eigen::Isometry3d& wrist_to_camera, const Eigen::Isometry3d& base_to_target, + const Eigen::Isometry3d& base_to_wrist, const rct_optimizations::CameraIntrinsics& intr, const rct_image_tools::ModifiedCircleGridTarget& target, const cv::Mat& image, const rct_optimizations::CorrespondenceSet& corr) { // We want to compute the "positional error" as well // So first we compute the "camera to target" transform based on the calibration... - Eigen::Affine3d camera_to_target = (base_to_wrist * wrist_to_camera).inverse() * base_to_target; + Eigen::Isometry3d camera_to_target = (base_to_wrist * wrist_to_camera).inverse() * base_to_target; std::vector reprojections = rct_image_tools::getReprojections(camera_to_target, intr, target.points); cv::Mat frame = image.clone(); @@ -140,11 +140,11 @@ int main(int argc, char** argv) rct_ros_tools::printOptResults(opt_result.converged, opt_result.initial_cost_per_obs, opt_result.final_cost_per_obs); rct_ros_tools::printNewLine(); - Eigen::Affine3d c = opt_result.wrist_to_camera; + Eigen::Isometry3d c = opt_result.wrist_to_camera; rct_ros_tools::printTransform(c, "Wrist", "Camera", "WRIST TO CAMERA"); rct_ros_tools::printNewLine(); - Eigen::Affine3d t = opt_result.base_to_target; + Eigen::Isometry3d t = opt_result.base_to_target; rct_ros_tools::printTransform(t, "Base", "Target", "BASE TO TARGET"); rct_ros_tools::printNewLine(); diff --git a/rct_examples/src/tools/multi_static_camera_extrinsic.cpp b/rct_examples/src/tools/multi_static_camera_extrinsic.cpp index 248ace43..5b8bcabb 100644 --- a/rct_examples/src/tools/multi_static_camera_extrinsic.cpp +++ b/rct_examples/src/tools/multi_static_camera_extrinsic.cpp @@ -16,15 +16,15 @@ #include #include -static void reproject(const Eigen::Affine3d& base_to_target, - const std::vector& base_to_camera, +static void reproject(const Eigen::Isometry3d& base_to_target, + const std::vector& base_to_camera, const std::vector& intr, const rct_image_tools::ModifiedCircleGridTarget& target, const cv::Mat& image, const std::vector& corr) { - Eigen::Affine3d camera_to_target = base_to_camera[0].inverse() * base_to_target; + Eigen::Isometry3d camera_to_target = base_to_camera[0].inverse() * base_to_target; std::vector reprojections = rct_image_tools::getReprojections(camera_to_target, intr[0], target.points); cv::Mat before_frame = image.clone(); @@ -44,7 +44,7 @@ static void reproject(const Eigen::Affine3d& base_to_target, rct_ros_tools::printTransform(camera_to_target, "Camera 0", "Target", "CAMERA 0 TO TARGET"); rct_ros_tools::printNewLine(); - Eigen::Affine3d result_camera_to_target = base_to_camera[0].inverse() * r.base_to_target; + Eigen::Isometry3d result_camera_to_target = base_to_camera[0].inverse() * r.base_to_target; rct_ros_tools::printTransform(result_camera_to_target, "Camera 0", "Target", "PNP"); rct_ros_tools::printNewLine(); @@ -174,7 +174,7 @@ int main(int argc, char** argv) rct_ros_tools::printOptResults(opt_result.converged, opt_result.initial_cost_per_obs, opt_result.final_cost_per_obs); rct_ros_tools::printNewLine(); - Eigen::Affine3d t = opt_result.wrist_to_target; + Eigen::Isometry3d t = opt_result.wrist_to_target; rct_ros_tools::printTransform(t, "Wrist", "Target", "WRIST TO TARGET"); rct_ros_tools::printNewLine(); @@ -196,8 +196,8 @@ int main(int argc, char** argv) for (std::size_t i = 0; i < maybe_data_set[0].images.size(); ++i) { std::vector corr_set; - std::vector base_to_camera; - Eigen::Affine3d base_to_wrist; + std::vector base_to_camera; + Eigen::Isometry3d base_to_wrist; std::vector intr; cv::Mat image; diff --git a/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp b/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp index b7d235cb..e5828853 100644 --- a/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp +++ b/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp @@ -16,15 +16,15 @@ #include #include -static void reproject(const Eigen::Affine3d& base_to_target, - const std::vector& base_to_camera, +static void reproject(const Eigen::Isometry3d& base_to_target, + const std::vector& base_to_camera, const std::vector& intr, const rct_image_tools::ModifiedCircleGridTarget& target, const cv::Mat& image, const std::vector& corr) { - Eigen::Affine3d camera_to_target = base_to_camera[0].inverse() * base_to_target; + Eigen::Isometry3d camera_to_target = base_to_camera[0].inverse() * base_to_target; std::vector reprojections = rct_image_tools::getReprojections(camera_to_target, intr[0], target.points); cv::Mat before_frame = image.clone(); @@ -43,7 +43,7 @@ static void reproject(const Eigen::Affine3d& base_to_target, rct_ros_tools::printTransform(camera_to_target, "Camera 0", "Target", "CAMERA 0 TO TARGET"); rct_ros_tools::printNewLine(); - Eigen::Affine3d result_camera_to_target = base_to_camera[0].inverse() * r.base_to_target; + Eigen::Isometry3d result_camera_to_target = base_to_camera[0].inverse() * r.base_to_target; rct_ros_tools::printTransformDiff(camera_to_target, result_camera_to_target, "Camera 0", "Target", "PNP"); rct_ros_tools::printNewLine(); @@ -141,7 +141,7 @@ int main(int argc, char** argv) ROS_WARN_STREAM("Unable to load target file from the 'target_path' parameter"); } - Eigen::Affine3d wrist_to_target; + Eigen::Isometry3d wrist_to_target; if (!rct_ros_tools::loadPose(pnh, "wrist_to_target_guess", wrist_to_target)) { ROS_WARN_STREAM("Unable to load guess for wrist to target from the 'wrist_to_target_guess' parameter struct"); @@ -185,7 +185,7 @@ int main(int argc, char** argv) // Load the data set path from ROS param std::string param_base = "camera_" + std::to_string(c); - Eigen::Affine3d t = opt_result.base_to_camera[c]; + Eigen::Isometry3d t = opt_result.base_to_camera[c]; rct_ros_tools::printTransform(t, "Base", "Camera (" + param_base + ")", "Base To Camera (" + param_base + ")"); rct_ros_tools::printNewLine(); @@ -208,7 +208,7 @@ int main(int argc, char** argv) rct_ros_tools::printOptResults(opt_wrist_only_result.converged, opt_wrist_only_result.initial_cost_per_obs, opt_wrist_only_result.final_cost_per_obs); rct_ros_tools::printNewLine(); - Eigen::Affine3d t = opt_wrist_only_result.wrist_to_target; + Eigen::Isometry3d t = opt_wrist_only_result.wrist_to_target; rct_ros_tools::printTransform(t, "Wrist", "Target", "Wrist to Target"); rct_ros_tools::printNewLine(); @@ -231,8 +231,8 @@ int main(int argc, char** argv) for (std::size_t i = 0; i < problem_wrist_def.wrist_poses.size(); ++i) { std::vector corr_set; - std::vector base_to_camera; - Eigen::Affine3d base_to_target; + std::vector base_to_camera; + Eigen::Isometry3d base_to_target; std::vector intr; corr_set.reserve(num_of_cameras); diff --git a/rct_examples/src/tools/solve_multi_camera_pnp.cpp b/rct_examples/src/tools/solve_multi_camera_pnp.cpp index 6560628f..4e35b1dd 100644 --- a/rct_examples/src/tools/solve_multi_camera_pnp.cpp +++ b/rct_examples/src/tools/solve_multi_camera_pnp.cpp @@ -17,15 +17,15 @@ #include #include -static void reproject(const Eigen::Affine3d& base_to_target, - const std::vector& base_to_camera, +static void reproject(const Eigen::Isometry3d& base_to_target, + const std::vector& base_to_camera, const std::vector& intr, const rct_image_tools::ModifiedCircleGridTarget& target, const cv::Mat& image, const std::vector& corr) { - Eigen::Affine3d camera_to_target = base_to_camera[0].inverse() * base_to_target; + Eigen::Isometry3d camera_to_target = base_to_camera[0].inverse() * base_to_target; std::vector reprojections = rct_image_tools::getReprojections(camera_to_target, intr[0], target.points); cv::Mat before_frame = image.clone(); @@ -53,7 +53,7 @@ static void reproject(const Eigen::Affine3d& base_to_target, rct_ros_tools::printTransformDiff(base_to_target, r.base_to_target, "Base", "Target", "PNP Diff"); rct_ros_tools::printNewLine(); - Eigen::Affine3d result_camera_to_target = base_to_camera[0].inverse() * r.base_to_target; + Eigen::Isometry3d result_camera_to_target = base_to_camera[0].inverse() * r.base_to_target; reprojections = rct_image_tools::getReprojections(result_camera_to_target, intr[0], target.points); cv::Mat after_frame = image.clone(); @@ -77,7 +77,7 @@ int main(int argc, char** argv) } std::size_t num_of_cameras = camera_count; - std::vector base_to_camera; + std::vector base_to_camera; std::vector intr; std::vector data_path; std::string target_path; diff --git a/rct_examples/src/tools/solve_pnp.cpp b/rct_examples/src/tools/solve_pnp.cpp index 4f22aab3..cb433c55 100644 --- a/rct_examples/src/tools/solve_pnp.cpp +++ b/rct_examples/src/tools/solve_pnp.cpp @@ -14,7 +14,7 @@ #include #include -static Eigen::Affine3d solveCVPnP(const rct_optimizations::CameraIntrinsics& intr, +static Eigen::Isometry3d solveCVPnP(const rct_optimizations::CameraIntrinsics& intr, const rct_image_tools::ModifiedCircleGridTarget& target, const std::vector& obs) { @@ -42,7 +42,7 @@ static Eigen::Affine3d solveCVPnP(const rct_optimizations::CameraIntrinsics& int cv::solvePnP(target_points, image_points, cam_matrix, cv::noArray(), rvec, tvec); Eigen::Vector3d rr (Eigen::Vector3d(rvec.at(0, 0), rvec.at(1, 0), rvec.at(2, 0))); - Eigen::Affine3d result(Eigen::AngleAxisd(rr.norm(), rr.normalized())); + Eigen::Isometry3d result(Eigen::AngleAxisd(rr.norm(), rr.normalized())); result.translation() = Eigen::Vector3d(tvec.at(0, 0), tvec.at(1, 0), tvec.at(2, 0)); rct_ros_tools::printTransform(result, "Camera", "Target", "OpenCV solvePNP"); @@ -98,7 +98,7 @@ int main(int argc, char** argv) solveCVPnP(intr, target, *maybe_obs); // Solve with some native RCT function (for learning) - Eigen::Affine3d guess = Eigen::Affine3d::Identity(); + Eigen::Isometry3d guess = Eigen::Isometry3d::Identity(); guess = guess * Eigen::Translation3d(0,0,0.1) * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()); rct_optimizations::PnPProblem params; diff --git a/rct_examples/src/tools/static_camera_extrinsic.cpp b/rct_examples/src/tools/static_camera_extrinsic.cpp index 877a55b5..3a7bf374 100644 --- a/rct_examples/src/tools/static_camera_extrinsic.cpp +++ b/rct_examples/src/tools/static_camera_extrinsic.cpp @@ -15,12 +15,12 @@ #include #include -static void reproject(const Eigen::Affine3d& wrist_to_target, const Eigen::Affine3d& base_to_camera, - const Eigen::Affine3d& base_to_wrist, const rct_optimizations::CameraIntrinsics& intr, +static void reproject(const Eigen::Isometry3d& wrist_to_target, const Eigen::Isometry3d& base_to_camera, + const Eigen::Isometry3d& base_to_wrist, const rct_optimizations::CameraIntrinsics& intr, const rct_image_tools::ModifiedCircleGridTarget& target, const cv::Mat& image, const rct_optimizations::CorrespondenceSet& corr) { - Eigen::Affine3d camera_to_target = base_to_camera.inverse() * (base_to_wrist * wrist_to_target); + Eigen::Isometry3d camera_to_target = base_to_camera.inverse() * (base_to_wrist * wrist_to_target); std::vector reprojections = rct_image_tools::getReprojections(camera_to_target, intr, target.points); cv::Mat frame = image.clone(); @@ -144,11 +144,11 @@ int main(int argc, char** argv) rct_ros_tools::printOptResults(opt_result.converged, opt_result.initial_cost_per_obs, opt_result.final_cost_per_obs); rct_ros_tools::printNewLine(); - Eigen::Affine3d c = opt_result.base_to_camera; + Eigen::Isometry3d c = opt_result.base_to_camera; rct_ros_tools::printTransform(c, "Base", "Camera", "BASE TO CAMERA"); rct_ros_tools::printNewLine(); - Eigen::Affine3d t = opt_result.wrist_to_target; + Eigen::Isometry3d t = opt_result.wrist_to_target; rct_ros_tools::printTransform(t, "Base", "Camera", "BASE TO CAMERA"); rct_ros_tools::printNewLine(); diff --git a/rct_image_tools/include/rct_image_tools/image_utils.h b/rct_image_tools/include/rct_image_tools/image_utils.h index d040aa0e..adf815ea 100644 --- a/rct_image_tools/include/rct_image_tools/image_utils.h +++ b/rct_image_tools/include/rct_image_tools/image_utils.h @@ -17,7 +17,7 @@ namespace rct_image_tools * @return A vector of uv values in the image frame */ inline -std::vector getReprojections(const Eigen::Affine3d &camera_to_target, +std::vector getReprojections(const Eigen::Isometry3d &camera_to_target, const rct_optimizations::CameraIntrinsics &intr, const std::vector &target_points) { diff --git a/rct_optimizations/include/rct_optimizations/eigen_conversions.h b/rct_optimizations/include/rct_optimizations/eigen_conversions.h index 548eddf4..c0fd1f0a 100644 --- a/rct_optimizations/include/rct_optimizations/eigen_conversions.h +++ b/rct_optimizations/include/rct_optimizations/eigen_conversions.h @@ -7,9 +7,9 @@ namespace rct_optimizations { -Pose6d poseEigenToCal(const Eigen::Affine3d& pose); +Pose6d poseEigenToCal(const Eigen::Isometry3d& pose); -Eigen::Affine3d poseCalToEigen(const Pose6d& pose); +Eigen::Isometry3d poseCalToEigen(const Pose6d& pose); } diff --git a/rct_optimizations/include/rct_optimizations/experimental/camera_intrinsic.h b/rct_optimizations/include/rct_optimizations/experimental/camera_intrinsic.h index d6c03a1a..ce18ca9a 100644 --- a/rct_optimizations/include/rct_optimizations/experimental/camera_intrinsic.h +++ b/rct_optimizations/include/rct_optimizations/experimental/camera_intrinsic.h @@ -13,7 +13,7 @@ struct IntrinsicEstimationProblem CameraIntrinsics intrinsics_guess; bool use_extrinsic_guesses; - std::vector extrinsic_guesses; + std::vector extrinsic_guesses; }; struct IntrinsicEstimationResult @@ -25,7 +25,7 @@ struct IntrinsicEstimationResult CameraIntrinsics intrinsics; std::array distortions; - std::vector target_transforms; + std::vector target_transforms; }; IntrinsicEstimationResult optimize(const IntrinsicEstimationProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/experimental/multi_camera_pnp.h b/rct_optimizations/include/rct_optimizations/experimental/multi_camera_pnp.h index 826a35f5..25c5651f 100644 --- a/rct_optimizations/include/rct_optimizations/experimental/multi_camera_pnp.h +++ b/rct_optimizations/include/rct_optimizations/experimental/multi_camera_pnp.h @@ -26,7 +26,7 @@ struct MultiCameraPnPProblem /** @brief The "base frame" to "camera frame" transform; one for each camera. Should have same length as @e intr. */ - std::vector base_to_camera; + std::vector base_to_camera; /** @brief A sequence of observation sets corresponding to the image locations. * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target @@ -35,7 +35,7 @@ struct MultiCameraPnPProblem std::vector image_observations; /** @brief Your best guess for transforms, "base to target", for a given observation set taken.*/ - Eigen::Affine3d base_to_target_guess; + Eigen::Isometry3d base_to_target_guess; }; struct MultiCameraPnPResult @@ -62,7 +62,7 @@ struct MultiCameraPnPResult double final_cost_per_obs; /** @brief The final location of the target. */ - Eigen::Affine3d base_to_target; + Eigen::Isometry3d base_to_target; }; MultiCameraPnPResult optimize(const MultiCameraPnPProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/experimental/pnp.h b/rct_optimizations/include/rct_optimizations/experimental/pnp.h index 31e4ccd4..758224f9 100644 --- a/rct_optimizations/include/rct_optimizations/experimental/pnp.h +++ b/rct_optimizations/include/rct_optimizations/experimental/pnp.h @@ -11,7 +11,7 @@ struct PnPProblem rct_optimizations::CameraIntrinsics intr; CorrespondenceSet correspondences; - Eigen::Affine3d camera_to_target_guess; + Eigen::Isometry3d camera_to_target_guess; }; struct PnPResult @@ -20,7 +20,7 @@ struct PnPResult double initial_cost_per_obs; double final_cost_per_obs; - Eigen::Affine3d camera_to_target; + Eigen::Isometry3d camera_to_target; }; PnPResult optimize(const PnPProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/extrinsic_3d_camera_on_wrist.h b/rct_optimizations/include/rct_optimizations/extrinsic_3d_camera_on_wrist.h index 73dd5098..ac823c18 100644 --- a/rct_optimizations/include/rct_optimizations/extrinsic_3d_camera_on_wrist.h +++ b/rct_optimizations/include/rct_optimizations/extrinsic_3d_camera_on_wrist.h @@ -26,11 +26,11 @@ namespace rct_optimizations struct Extrinsic3DCameraOnWristProblem { - std::vector wrist_poses; + std::vector wrist_poses; std::vector observations; - Eigen::Affine3d base_to_target_guess; - Eigen::Affine3d wrist_to_camera_guess; + Eigen::Isometry3d base_to_target_guess; + Eigen::Isometry3d wrist_to_camera_guess; }; struct Extrinsic3DCameraOnWristResult @@ -39,8 +39,8 @@ struct Extrinsic3DCameraOnWristResult double initial_cost_per_obs; double final_cost_per_obs; - Eigen::Affine3d base_to_target; - Eigen::Affine3d wrist_to_camera; + Eigen::Isometry3d base_to_target; + Eigen::Isometry3d wrist_to_camera; }; Extrinsic3DCameraOnWristResult optimize(const Extrinsic3DCameraOnWristProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/extrinsic_camera_on_wrist.h b/rct_optimizations/include/rct_optimizations/extrinsic_camera_on_wrist.h index 7b404759..e902f861 100644 --- a/rct_optimizations/include/rct_optimizations/extrinsic_camera_on_wrist.h +++ b/rct_optimizations/include/rct_optimizations/extrinsic_camera_on_wrist.h @@ -59,7 +59,7 @@ struct ExtrinsicCameraOnWristProblem * @brief The transforms, "base to wrist", at which each observation set was taken. Should be * same size as @e image_observations. */ - std::vector wrist_poses; + std::vector wrist_poses; /** * @brief A sequence of observation sets corresponding to the image locations in @e wrist_poses. @@ -72,12 +72,12 @@ struct ExtrinsicCameraOnWristProblem * @brief Your best guess at the transform from "base frame" to "target frame". Should be static * as the robot moves. */ - Eigen::Affine3d base_to_target_guess; + Eigen::Isometry3d base_to_target_guess; /** * @brief Your best guess at teh transform from "wrist frame" to the camera optical frame. */ - Eigen::Affine3d wrist_to_camera_guess; + Eigen::Isometry3d wrist_to_camera_guess; }; struct ExtrinsicCameraOnWristResult @@ -106,12 +106,12 @@ struct ExtrinsicCameraOnWristResult /** * @brief The final calibrated result of "base frame" to "target frame". */ - Eigen::Affine3d base_to_target; + Eigen::Isometry3d base_to_target; /** * @brief The final calibrated result of "wrist frame" to camera optical frame. */ - Eigen::Affine3d wrist_to_camera; + Eigen::Isometry3d wrist_to_camera; }; ExtrinsicCameraOnWristResult optimize(const ExtrinsicCameraOnWristProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera.h b/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera.h index 716f4192..18074c4d 100644 --- a/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera.h +++ b/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera.h @@ -32,7 +32,7 @@ struct ExtrinsicMultiStaticCameraMovingTargetProblem * vector is for each camera, the inner vector is the poses valid for that camera. This inner * vector should match the inner vector of @e image_observations in size. */ - std::vector> wrist_poses; + std::vector> wrist_poses; /** @brief A sequence of observation sets corresponding to the image locations in @e wrist_poses. * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target @@ -42,10 +42,10 @@ struct ExtrinsicMultiStaticCameraMovingTargetProblem std::vector> image_observations; /** @brief Your best guess at the "wrist frame" to "target frame" transform */ - Eigen::Affine3d wrist_to_target_guess; + Eigen::Isometry3d wrist_to_target_guess; /** @brief Your best guess at the "base frame" to "camera frame" transform; one for each camera */ - std::vector base_to_camera_guess; + std::vector base_to_camera_guess; }; struct ExtrinsicMultiStaticCameraMovingTargetResult @@ -74,12 +74,12 @@ struct ExtrinsicMultiStaticCameraMovingTargetResult /** * @brief The final calibrated result of "wrist frame" to "target frame". */ - Eigen::Affine3d wrist_to_target; + Eigen::Isometry3d wrist_to_target; /** * @brief The final calibrated result of "base frame" to "camera optical frame". */ - std::vector base_to_camera; + std::vector base_to_camera; }; ExtrinsicMultiStaticCameraMovingTargetResult optimize(const ExtrinsicMultiStaticCameraMovingTargetProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_only.h b/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_only.h index 7a69d38c..3c2237ce 100644 --- a/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_only.h +++ b/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_only.h @@ -37,7 +37,7 @@ struct ExtrinsicMultiStaticCameraOnlyProblem * The vector is the poses valid for each camera. This vector should match the inner * vector of @e image_observations in size. */ - std::vector base_to_target_guess; + std::vector base_to_target_guess; /** @brief A sequence of observation sets corresponding to the image locations in @e base_to_target_guess. * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target @@ -47,7 +47,7 @@ struct ExtrinsicMultiStaticCameraOnlyProblem std::vector> image_observations; /** @brief Your best guess at the "base frame" to "camera frame" transform; one for each camera */ - std::vector base_to_camera_guess; + std::vector base_to_camera_guess; }; struct ExtrinsicMultiStaticCameraOnlyResult @@ -74,10 +74,10 @@ struct ExtrinsicMultiStaticCameraOnlyResult double final_cost_per_obs; /** @brief The final calibrated result of "base frame" to "target frame". */ - std::vector base_to_target; + std::vector base_to_target; /** @brief The final calibrated result of "base frame" to "camera optical frame". */ - std::vector base_to_camera; + std::vector base_to_camera; }; ExtrinsicMultiStaticCameraOnlyResult optimize(const ExtrinsicMultiStaticCameraOnlyProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_wrist_only.h b/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_wrist_only.h index 5465ad48..e3218e0f 100644 --- a/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_wrist_only.h +++ b/rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_wrist_only.h @@ -32,7 +32,7 @@ struct ExtrinsicMultiStaticCameraMovingTargetWristOnlyProblem * The vector is the poses valid for each camera. This vector should match the inner * vector of @e image_observations in size. */ - std::vector wrist_poses; + std::vector wrist_poses; /** @brief A sequence of observation sets corresponding to the image locations in @e wrist_poses. * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target @@ -42,13 +42,13 @@ struct ExtrinsicMultiStaticCameraMovingTargetWristOnlyProblem std::vector> image_observations; /** @brief Your best guess at the "wrist frame" to "target frame" transform */ - Eigen::Affine3d wrist_to_target_guess; + Eigen::Isometry3d wrist_to_target_guess; /** @brief Your best guess at the "base frame" to "camera frame" transform; one for each camera. * Also it assumses the relationship between the cameras is correct and fixed, so it will * calibrating the set of cameras using a single transformation. */ - std::vector base_to_camera_guess; + std::vector base_to_camera_guess; }; struct ExtrinsicMultiStaticCameraMovingTargetWristOnlyResult @@ -77,12 +77,12 @@ struct ExtrinsicMultiStaticCameraMovingTargetWristOnlyResult /** * @brief The final calibrated result of "wrist frame" to "target frame". */ - Eigen::Affine3d wrist_to_target; + Eigen::Isometry3d wrist_to_target; /** * @brief The final calibrated result of "base frame" to "camera optical frame". */ - std::vector base_to_camera; + std::vector base_to_camera; }; ExtrinsicMultiStaticCameraMovingTargetWristOnlyResult optimize(const ExtrinsicMultiStaticCameraMovingTargetWristOnlyProblem& params); diff --git a/rct_optimizations/include/rct_optimizations/extrinsic_static_camera.h b/rct_optimizations/include/rct_optimizations/extrinsic_static_camera.h index 865d64fd..81e8ae7c 100644 --- a/rct_optimizations/include/rct_optimizations/extrinsic_static_camera.h +++ b/rct_optimizations/include/rct_optimizations/extrinsic_static_camera.h @@ -56,7 +56,7 @@ struct ExtrinsicStaticCameraMovingTargetProblem /** @brief The transforms, "base to wrist", at which each observation set was taken. Should be * same size as @e image_observations. */ - std::vector wrist_poses; + std::vector wrist_poses; /** @brief A sequence of observation sets corresponding to the image locations in @e wrist_poses. * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target @@ -65,10 +65,10 @@ struct ExtrinsicStaticCameraMovingTargetProblem std::vector image_observations; /** @brief Your best guess at the "wrist frame" to "target frame" transform */ - Eigen::Affine3d wrist_to_target_guess; + Eigen::Isometry3d wrist_to_target_guess; /** @brief Your best guess at the "base frame" to "camera frame" transform */ - Eigen::Affine3d base_to_camera_guess; + Eigen::Isometry3d base_to_camera_guess; }; struct ExtrinsicStaticCameraMovingTargetResult @@ -97,12 +97,12 @@ struct ExtrinsicStaticCameraMovingTargetResult /** * @brief The final calibrated result of "wrist frame" to "target frame". */ - Eigen::Affine3d wrist_to_target; + Eigen::Isometry3d wrist_to_target; /** * @brief The final calibrated result of "base frame" to "camera optical frame". */ - Eigen::Affine3d base_to_camera; + Eigen::Isometry3d base_to_camera; }; ExtrinsicStaticCameraMovingTargetResult optimize(const ExtrinsicStaticCameraMovingTargetProblem& params); diff --git a/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp b/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp index 6765fd49..e76b0366 100644 --- a/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp +++ b/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp @@ -171,7 +171,7 @@ class IntrinsicCostFunction static rct_optimizations::Pose6d guessInitialPose() { - Eigen::Affine3d guess = Eigen::Affine3d::Identity(); + Eigen::Isometry3d guess = Eigen::Isometry3d::Identity(); guess = guess * Eigen::Translation3d(0,0,0.5) * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()); return rct_optimizations::poseEigenToCal(guess); } diff --git a/rct_optimizations/src/rct_optimizations/eigen_conversions.cpp b/rct_optimizations/src/rct_optimizations/eigen_conversions.cpp index ce2ff958..eea2993e 100644 --- a/rct_optimizations/src/rct_optimizations/eigen_conversions.cpp +++ b/rct_optimizations/src/rct_optimizations/eigen_conversions.cpp @@ -1,6 +1,6 @@ #include "rct_optimizations/eigen_conversions.h" -rct_optimizations::Pose6d rct_optimizations::poseEigenToCal(const Eigen::Affine3d& pose) +rct_optimizations::Pose6d rct_optimizations::poseEigenToCal(const Eigen::Isometry3d& pose) { Pose6d p; p.x() = pose.translation().x(); @@ -16,9 +16,9 @@ rct_optimizations::Pose6d rct_optimizations::poseEigenToCal(const Eigen::Affine3 return p; } -Eigen::Affine3d rct_optimizations::poseCalToEigen(const rct_optimizations::Pose6d& pose) +Eigen::Isometry3d rct_optimizations::poseCalToEigen(const rct_optimizations::Pose6d& pose) { - Eigen::Affine3d p = Eigen::Affine3d::Identity(); + Eigen::Isometry3d p = Eigen::Isometry3d::Identity(); p.translation().x() = pose.x(); p.translation().y() = pose.y(); p.translation().z() = pose.z(); diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_3d_camera_on_wrist.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_3d_camera_on_wrist.cpp index b1484e5c..5670d2ca 100644 --- a/rct_optimizations/src/rct_optimizations/extrinsic_3d_camera_on_wrist.cpp +++ b/rct_optimizations/src/rct_optimizations/extrinsic_3d_camera_on_wrist.cpp @@ -27,7 +27,7 @@ class ObservationCost * @param wrist_pose * @param point_in_target */ - ObservationCost(const Eigen::Vector3d& obs, const Eigen::Affine3d& wrist_to_base, + ObservationCost(const Eigen::Vector3d& obs, const Eigen::Isometry3d& wrist_to_base, const Eigen::Vector3d& point_in_target) : obs_(obs), wrist_pose_(poseEigenToCal(wrist_to_base)), target_pt_(point_in_target) {} diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_camera_on_wrist.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_camera_on_wrist.cpp index 974390cf..c500cccb 100644 --- a/rct_optimizations/src/rct_optimizations/extrinsic_camera_on_wrist.cpp +++ b/rct_optimizations/src/rct_optimizations/extrinsic_camera_on_wrist.cpp @@ -26,7 +26,7 @@ class ReprojectionCost * @param wrist_pose * @param point_in_target */ - ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Affine3d& wrist_to_base, + ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Isometry3d& wrist_to_base, const Eigen::Vector3d& point_in_target) : obs_(obs), intr_(intr), wrist_pose_(poseEigenToCal(wrist_to_base)), target_pt_(point_in_target) { diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp index b79d471d..cd32e7fc 100644 --- a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp +++ b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp @@ -14,7 +14,7 @@ namespace class ReprojectionCost { public: - ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Affine3d& base_to_wrist, + ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Isometry3d& base_to_wrist, const Eigen::Vector3d& point_in_target) : obs_(obs), intr_(intr), wrist_pose_(poseEigenToCal(base_to_wrist)), target_pt_(point_in_target) {} diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_only.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_only.cpp index e36af319..7bfa59cb 100644 --- a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_only.cpp +++ b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_only.cpp @@ -58,7 +58,7 @@ class ReprojectionFreeCameraCost class ReprojectionFixedCameraCost { public: - ReprojectionFixedCameraCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Affine3d& base_to_camera, const Eigen::Vector3d& point_in_target) + ReprojectionFixedCameraCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Isometry3d& base_to_camera, const Eigen::Vector3d& point_in_target) : obs_(obs), intr_(intr), camera_to_base_(poseEigenToCal(base_to_camera.inverse())), target_pt_(point_in_target) {} diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_wrist_only.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_wrist_only.cpp index cc06ed89..89d9c704 100644 --- a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_wrist_only.cpp +++ b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera_wrist_only.cpp @@ -14,8 +14,8 @@ namespace class ReprojectionCost { public: - ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Affine3d& base_to_wrist, - const Eigen::Affine3d& base_to_camera, const Eigen::Vector3d& point_in_target) + ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Isometry3d& base_to_wrist, + const Eigen::Isometry3d& base_to_camera, const Eigen::Vector3d& point_in_target) : obs_(obs), intr_(intr), wrist_pose_(poseEigenToCal(base_to_wrist)), camera_to_base_orig_(poseEigenToCal(base_to_camera.inverse())), target_pt_(point_in_target) {} @@ -70,7 +70,7 @@ rct_optimizations::optimize(const rct_optimizations::ExtrinsicMultiStaticCameraM { Pose6d internal_wrist_to_target = poseEigenToCal(params.wrist_to_target_guess); - Pose6d internal_camera_to_base_correction = poseEigenToCal(Eigen::Affine3d::Identity()); + Pose6d internal_camera_to_base_correction = poseEigenToCal(Eigen::Isometry3d::Identity()); ceres::Problem problem; @@ -109,7 +109,7 @@ rct_optimizations::optimize(const rct_optimizations::ExtrinsicMultiStaticCameraM result.base_to_camera.resize(params.base_to_camera_guess.size()); result.converged = summary.termination_type == ceres::CONVERGENCE; - Eigen::Affine3d base_to_camera_correction = poseCalToEigen(internal_camera_to_base_correction).inverse(); + Eigen::Isometry3d base_to_camera_correction = poseCalToEigen(internal_camera_to_base_correction).inverse(); for (std::size_t i = 0; i < params.base_to_camera_guess.size(); ++i) result.base_to_camera[i] = base_to_camera_correction * params.base_to_camera_guess[i]; diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_static_camera.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_static_camera.cpp index 194218c6..3b5704cd 100644 --- a/rct_optimizations/src/rct_optimizations/extrinsic_static_camera.cpp +++ b/rct_optimizations/src/rct_optimizations/extrinsic_static_camera.cpp @@ -14,7 +14,7 @@ namespace class ReprojectionCost { public: - ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Affine3d& base_to_wrist, + ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, const Eigen::Isometry3d& base_to_wrist, const Eigen::Vector3d& point_in_target) : obs_(obs), intr_(intr), wrist_pose_(poseEigenToCal(base_to_wrist)), target_pt_(point_in_target) {} diff --git a/rct_optimizations/src/rct_optimizations/multi_camera_pnp.cpp b/rct_optimizations/src/rct_optimizations/multi_camera_pnp.cpp index c1f79e29..73cf55aa 100644 --- a/rct_optimizations/src/rct_optimizations/multi_camera_pnp.cpp +++ b/rct_optimizations/src/rct_optimizations/multi_camera_pnp.cpp @@ -15,7 +15,7 @@ class ReprojectionCost public: ReprojectionCost(const Eigen::Vector2d& obs, const CameraIntrinsics& intr, - const Eigen::Affine3d& camera_to_base, + const Eigen::Isometry3d& camera_to_base, const Eigen::Vector3d& point_in_target) : obs_(obs), intr_(intr), diff --git a/rct_optimizations/test/conversion_utest.cpp b/rct_optimizations/test/conversion_utest.cpp index 6ae888f0..cdc4afd4 100644 --- a/rct_optimizations/test/conversion_utest.cpp +++ b/rct_optimizations/test/conversion_utest.cpp @@ -4,7 +4,7 @@ // Test the conversion of a pose from Eigen to internal calibration format and back TEST(EigenConversions, there_and_back_identity) { - const auto e_pose = Eigen::Affine3d::Identity(); + const auto e_pose = Eigen::Isometry3d::Identity(); const auto cal_pose = rct_optimizations::poseEigenToCal(e_pose); const auto recovered_e_pose = rct_optimizations::poseCalToEigen(cal_pose); @@ -15,7 +15,7 @@ TEST(EigenConversions, there_and_back_identity) // Test the conversion of a pose from Eigen to internal calibration format and back TEST(EigenConversions, there_and_back) { - auto e_pose = Eigen::Affine3d::Identity(); + auto e_pose = Eigen::Isometry3d::Identity(); e_pose = e_pose * Eigen::Translation3d(0.5, 0.75, 1.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()); const auto cal_pose = rct_optimizations::poseEigenToCal(e_pose); const auto recovered_e_pose = rct_optimizations::poseCalToEigen(cal_pose); diff --git a/rct_optimizations/test/extrinsic_camera_on_wrist_utest.cpp b/rct_optimizations/test/extrinsic_camera_on_wrist_utest.cpp index df46a507..96331914 100644 --- a/rct_optimizations/test/extrinsic_camera_on_wrist_utest.cpp +++ b/rct_optimizations/test/extrinsic_camera_on_wrist_utest.cpp @@ -13,8 +13,8 @@ static void printResults(const rct_optimizations::ExtrinsicCameraOnWristResult& std::cout << "Initial cost?: " << r.initial_cost_per_obs << "\n"; std::cout << "Final cost?: " << r.final_cost_per_obs << "\n"; - Eigen::Affine3d c = r.wrist_to_camera; - Eigen::Affine3d t = r.base_to_target; + Eigen::Isometry3d c = r.wrist_to_camera; + Eigen::Isometry3d t = r.base_to_target; std::cout << "Wrist to Camera:\n"; std::cout << c.matrix() << "\n"; @@ -36,10 +36,10 @@ void run_test(InitialConditions condition) auto grid = rct_optimizations::test::makeTarget(5, 7, 0.025); // Define the "true" conditions - auto true_base_to_target = Eigen::Affine3d::Identity(); + auto true_base_to_target = Eigen::Isometry3d::Identity(); true_base_to_target.translation() = Eigen::Vector3d(1.0, 0, 0); - auto true_wrist_to_camera = Eigen::Affine3d::Identity(); + auto true_wrist_to_camera = Eigen::Isometry3d::Identity(); true_wrist_to_camera.translation() = Eigen::Vector3d(0.05, 0, 0.1); true_wrist_to_camera.linear() << 0, 0, 1, -1, 0, 0, @@ -47,17 +47,17 @@ void run_test(InitialConditions condition) // Create some number of "test" images... // We'll take pictures in a grid above the origin of the target - std::vector wrist_poses; + std::vector wrist_poses; std::vector correspondences; for (int i = -5; i < 5; ++i) { for (int j = -5; j < 5; ++j) { Eigen::Vector3d center_point = true_base_to_target.translation() + Eigen::Vector3d(i * 0.025, j * 0.025, 1.0); - Eigen::Affine3d camera_pose = rct_optimizations::test::lookat(center_point, + Eigen::Isometry3d camera_pose = rct_optimizations::test::lookat(center_point, true_base_to_target.translation(), Eigen::Vector3d(1, 0, 0)); - Eigen::Affine3d wrist_pose = camera_pose * true_wrist_to_camera.inverse(); + Eigen::Isometry3d wrist_pose = camera_pose * true_wrist_to_camera.inverse(); // Attempt to generate points std::vector image_obs; diff --git a/rct_optimizations/test/extrinsic_multi_static_camera_utest.cpp b/rct_optimizations/test/extrinsic_multi_static_camera_utest.cpp index 61b96adc..ca717976 100644 --- a/rct_optimizations/test/extrinsic_multi_static_camera_utest.cpp +++ b/rct_optimizations/test/extrinsic_multi_static_camera_utest.cpp @@ -25,7 +25,7 @@ void printResults(const rct_optimizations::ExtrinsicMultiStaticCameraMovingTarge std::cout << "Initial cost?: " << opt_result.initial_cost_per_obs << "\n"; std::cout << "Final cost?: " << opt_result.final_cost_per_obs << "\n"; - Eigen::Affine3d t = opt_result.wrist_to_target; + Eigen::Isometry3d t = opt_result.wrist_to_target; std::cout << "Wrist to Target:\n"; std::cout << t.matrix() << "\n"; @@ -34,7 +34,7 @@ void printResults(const rct_optimizations::ExtrinsicMultiStaticCameraMovingTarge { // Load the data set path from ROS param std::string param_base = "camera_" + std::to_string(c); - Eigen::Affine3d t = opt_result.base_to_camera[c]; + Eigen::Isometry3d t = opt_result.base_to_camera[c]; std::cout << "Base to Camera (" + param_base + "):\n"; std::cout << t.matrix() << "\n"; @@ -48,18 +48,18 @@ void printResults(const rct_optimizations::ExtrinsicMultiStaticCameraMovingTarge void addObservations(const Eigen::Vector3d& axis, const std::vector &target_points, - const std::vector &base_to_camera, - const Eigen::Affine3d &wrist_to_target, + const std::vector &base_to_camera, + const Eigen::Isometry3d &wrist_to_target, rct_optimizations::ExtrinsicMultiStaticCameraMovingTargetProblem &problem_def) { for (int i = -10; i <= 10; ++i) { - Eigen::Affine3d base_to_wrist; + Eigen::Isometry3d base_to_wrist; - Eigen::Affine3d link_1 = Eigen::Affine3d::Identity(); + Eigen::Isometry3d link_1 = Eigen::Isometry3d::Identity(); link_1.translation() = Eigen::Vector3d(0, 0, 0.25); - Eigen::Affine3d link_2(Eigen::AngleAxisd(i * M_PI/180.0, axis)); + Eigen::Isometry3d link_2(Eigen::AngleAxisd(i * M_PI/180.0, axis)); link_2.translation() = Eigen::Vector3d(0, 0, 0.25); base_to_wrist = link_1 * link_2; @@ -73,7 +73,7 @@ void addObservations(const Eigen::Vector3d& axis, { rct_optimizations::Correspondence2D3D pair; - Eigen::Affine3d target_to_camera = wrist_to_target.inverse() * base_to_wrist.inverse() * base_to_camera[c]; + Eigen::Isometry3d target_to_camera = wrist_to_target.inverse() * base_to_wrist.inverse() * base_to_camera[c]; Eigen::Vector3d in_camera = target_to_camera.inverse() * target_points[j]; double uv[2]; @@ -94,11 +94,11 @@ TEST(ExtrinsicMultiStaticCamera, single_camera) std::vector target_points; makePoints(5, 5, 0.015, target_points); - std::vector base_to_camera; - base_to_camera.push_back(Eigen::Affine3d(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()))); + std::vector base_to_camera; + base_to_camera.push_back(Eigen::Isometry3d(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()))); base_to_camera[0].translation() = Eigen::Vector3d(0, 0, 2.0); - Eigen::Affine3d wrist_to_target = Eigen::Affine3d::Identity(); + Eigen::Isometry3d wrist_to_target = Eigen::Isometry3d::Identity(); wrist_to_target.translation() = Eigen::Vector3d(0, 0, 0.25); problem_def.intr.resize(1); @@ -106,10 +106,10 @@ TEST(ExtrinsicMultiStaticCamera, single_camera) problem_def.wrist_poses.resize(1); problem_def.image_observations.resize(1); - problem_def.base_to_camera_guess[0] = Eigen::Affine3d(Eigen::AngleAxisd(M_PI + 0.01, Eigen::Vector3d::UnitX())); + problem_def.base_to_camera_guess[0] = Eigen::Isometry3d(Eigen::AngleAxisd(M_PI + 0.01, Eigen::Vector3d::UnitX())); problem_def.base_to_camera_guess[0].translation() = Eigen::Vector3d(0.01, 0.001, 1.99); - problem_def.wrist_to_target_guess = Eigen::Affine3d::Identity(); + problem_def.wrist_to_target_guess = Eigen::Isometry3d::Identity(); problem_def.wrist_to_target_guess.translation() = Eigen::Vector3d(0.001, 0.001, 0.26); problem_def.intr[0].fx() = 1411.0; @@ -143,13 +143,13 @@ TEST(ExtrinsicMultiStaticCamera, two_cameras) std::vector target_points; makePoints(5, 5, 0.015, target_points); - std::vector base_to_camera; - Eigen::Affine3d wrist_to_target = Eigen::Affine3d::Identity(); + std::vector base_to_camera; + Eigen::Isometry3d wrist_to_target = Eigen::Isometry3d::Identity(); base_to_camera.resize(2); - base_to_camera[0] = Eigen::Affine3d(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + base_to_camera[0] = Eigen::Isometry3d(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); base_to_camera[0].translation() = Eigen::Vector3d(-0.1, 0, 2.0); - base_to_camera[1] = Eigen::Affine3d(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + base_to_camera[1] = Eigen::Isometry3d(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); base_to_camera[1].translation() = Eigen::Vector3d(0.1, 0, 2.0); wrist_to_target.translation() = Eigen::Vector3d(0, 0, 0.25); @@ -159,12 +159,12 @@ TEST(ExtrinsicMultiStaticCamera, two_cameras) problem_def.wrist_poses.resize(2); problem_def.image_observations.resize(2); - problem_def.base_to_camera_guess[0] = Eigen::Affine3d(Eigen::AngleAxisd(M_PI + 0.01, Eigen::Vector3d::UnitX())); + problem_def.base_to_camera_guess[0] = Eigen::Isometry3d(Eigen::AngleAxisd(M_PI + 0.01, Eigen::Vector3d::UnitX())); problem_def.base_to_camera_guess[0].translation() = Eigen::Vector3d(-0.101, 0.001, 1.99); - problem_def.base_to_camera_guess[1] = Eigen::Affine3d(Eigen::AngleAxisd(M_PI + 0.02, Eigen::Vector3d::UnitX())); + problem_def.base_to_camera_guess[1] = Eigen::Isometry3d(Eigen::AngleAxisd(M_PI + 0.02, Eigen::Vector3d::UnitX())); problem_def.base_to_camera_guess[1].translation() = Eigen::Vector3d(0.101, 0.001, 1.99); - problem_def.wrist_to_target_guess = Eigen::Affine3d::Identity(); + problem_def.wrist_to_target_guess = Eigen::Isometry3d::Identity(); problem_def.wrist_to_target_guess.translation() = Eigen::Vector3d(0.001, 0.001, 0.26); problem_def.intr[0].fx() = 1411.0; diff --git a/rct_optimizations/test/include/rct_optimizations_tests/observation_creator.h b/rct_optimizations/test/include/rct_optimizations_tests/observation_creator.h index 0262709b..f1c0a6a6 100644 --- a/rct_optimizations/test/include/rct_optimizations_tests/observation_creator.h +++ b/rct_optimizations/test/include/rct_optimizations_tests/observation_creator.h @@ -9,7 +9,7 @@ namespace test { // Given a virtual camera position, produce the expected 2D observations -bool project(const Eigen::Affine3d& camera_pose, const Eigen::Affine3d& target_pose, +bool project(const Eigen::Isometry3d& camera_pose, const Eigen::Isometry3d& target_pose, const Camera& camera, const Target& target, std::vector& out_observations); /** @@ -18,7 +18,7 @@ bool project(const Eigen::Affine3d& camera_pose, const Eigen::Affine3d& target_p * @param eye A point that the camera is looking at * @param up The upvector in world-space */ -Eigen::Affine3d lookat(const Eigen::Vector3d& origin, const Eigen::Vector3d& eye, const Eigen::Vector3d& up); +Eigen::Isometry3d lookat(const Eigen::Vector3d& origin, const Eigen::Vector3d& eye, const Eigen::Vector3d& up); } } diff --git a/rct_optimizations/test/include/rct_optimizations_tests/utilities.h b/rct_optimizations/test/include/rct_optimizations_tests/utilities.h index 8ea1509e..168be509 100644 --- a/rct_optimizations/test/include/rct_optimizations_tests/utilities.h +++ b/rct_optimizations/test/include/rct_optimizations_tests/utilities.h @@ -32,7 +32,7 @@ Target makeTarget(int rows, int cols, double spacing); // Helper CorrespondenceSet zip(const Target& target, const std::vector& image_obs); -Eigen::Affine3d perturbPose(const Eigen::Affine3d& pose, double spatial_noise, double angle_noise); +Eigen::Isometry3d perturbPose(const Eigen::Isometry3d& pose, double spatial_noise, double angle_noise); } } diff --git a/rct_optimizations/test/src/observation_creator.cpp b/rct_optimizations/test/src/observation_creator.cpp index 00a20a60..80808644 100644 --- a/rct_optimizations/test/src/observation_creator.cpp +++ b/rct_optimizations/test/src/observation_creator.cpp @@ -13,11 +13,11 @@ static bool projectAndTest(const Eigen::Vector3d& pt_in_camera, const rct_optimi else return true; } -bool rct_optimizations::test::project(const Eigen::Affine3d& camera_pose, const Eigen::Affine3d& target_pose, +bool rct_optimizations::test::project(const Eigen::Isometry3d& camera_pose, const Eigen::Isometry3d& target_pose, const test::Camera& camera, const test::Target& target, std::vector& out_observations) { - Eigen::Affine3d to_camera = camera_pose.inverse() * target_pose; + Eigen::Isometry3d to_camera = camera_pose.inverse() * target_pose; std::vector obs; // Loop over points @@ -44,13 +44,13 @@ bool rct_optimizations::test::project(const Eigen::Affine3d& camera_pose, const return true; } -Eigen::Affine3d rct_optimizations::test::lookat(const Eigen::Vector3d& origin, const Eigen::Vector3d& eye, const Eigen::Vector3d& up) +Eigen::Isometry3d rct_optimizations::test::lookat(const Eigen::Vector3d& origin, const Eigen::Vector3d& eye, const Eigen::Vector3d& up) { Eigen::Vector3d z = (eye - origin).normalized(); Eigen::Vector3d x = z.cross(up).normalized(); Eigen::Vector3d y = z.cross(x).normalized(); - auto p = Eigen::Affine3d::Identity(); + auto p = Eigen::Isometry3d::Identity(); p.translation() = origin; p.matrix().col(0).head<3>() = x; p.matrix().col(1).head<3>() = y; diff --git a/rct_optimizations/test/src/utilities.cpp b/rct_optimizations/test/src/utilities.cpp index 03991f16..1a16e85a 100644 --- a/rct_optimizations/test/src/utilities.cpp +++ b/rct_optimizations/test/src/utilities.cpp @@ -56,7 +56,7 @@ rct_optimizations::CorrespondenceSet rct_optimizations::test::zip(const rct_opti return out; } -Eigen::Affine3d rct_optimizations::test::perturbPose(const Eigen::Affine3d& pose, double spatial_noise, double angle_noise) +Eigen::Isometry3d rct_optimizations::test::perturbPose(const Eigen::Isometry3d& pose, double spatial_noise, double angle_noise) { std::random_device dev; std::default_random_engine eng (dev()); @@ -71,7 +71,7 @@ Eigen::Affine3d rct_optimizations::test::perturbPose(const Eigen::Affine3d& pose double angle = angle_dist(eng); - Eigen::Affine3d new_pose = pose * Eigen::Translation3d(translation) * Eigen::AngleAxisd(angle, rot_axis); + Eigen::Isometry3d new_pose = pose * Eigen::Translation3d(translation) * Eigen::AngleAxisd(angle, rot_axis); return new_pose; } diff --git a/rct_ros_tools/include/rct_ros_tools/data_set.h b/rct_ros_tools/include/rct_ros_tools/data_set.h index e77a29aa..5310007c 100644 --- a/rct_ros_tools/include/rct_ros_tools/data_set.h +++ b/rct_ros_tools/include/rct_ros_tools/data_set.h @@ -13,7 +13,7 @@ namespace rct_ros_tools struct ExtrinsicDataSet { std::vector images; - std::vector tool_poses; + std::vector tool_poses; }; boost::optional parseFromFile(const std::string& path); diff --git a/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h b/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h index 3e36e92c..e2b31f61 100644 --- a/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h +++ b/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h @@ -13,8 +13,8 @@ bool loadTarget(const std::string& path, rct_image_tools::ModifiedCircleGridTarg bool loadIntrinsics(const ros::NodeHandle& nh, const std::string& key, rct_optimizations::CameraIntrinsics& intr); -bool loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Affine3d& pose); -bool loadPose(const std::string& path, Eigen::Affine3d& pose); +bool loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Isometry3d& pose); +bool loadPose(const std::string& path, Eigen::Isometry3d& pose); } diff --git a/rct_ros_tools/include/rct_ros_tools/print_utils.h b/rct_ros_tools/include/rct_ros_tools/print_utils.h index 03ba1cf0..80670189 100644 --- a/rct_ros_tools/include/rct_ros_tools/print_utils.h +++ b/rct_ros_tools/include/rct_ros_tools/print_utils.h @@ -86,7 +86,7 @@ void printTitle(const std::string& title, int width = 80) } inline -void printTransform(const Eigen::Affine3d &transform, const std::string &parent_frame, const std::string &child_frame, const std::string &description) +void printTransform(const Eigen::Isometry3d &transform, const std::string &parent_frame, const std::string &child_frame, const std::string &description) { std::cout << description << ":" << std::endl << transform.matrix() << std::endl << std::endl; std::cout << "--- URDF Format " << parent_frame << " to " << child_frame << " ---" << std::endl; @@ -98,9 +98,9 @@ void printTransform(const Eigen::Affine3d &transform, const std::string &parent_ } inline -void printTransformDiff(const Eigen::Affine3d &transform1, const Eigen::Affine3d &transform2, const std::string &parent_frame, const std::string &child_frame, const std::string &description) +void printTransformDiff(const Eigen::Isometry3d &transform1, const Eigen::Isometry3d &transform2, const std::string &parent_frame, const std::string &child_frame, const std::string &description) { - Eigen::Affine3d delta = transform1.inverse() * transform2; + Eigen::Isometry3d delta = transform1.inverse() * transform2; Eigen::AngleAxisd aa (delta.linear()); Eigen::Vector3d rpy = delta.rotation().eulerAngles(2, 1, 0); diff --git a/rct_ros_tools/src/command_line_cal.cpp b/rct_ros_tools/src/command_line_cal.cpp index e1c7eecb..5443d99d 100644 --- a/rct_ros_tools/src/command_line_cal.cpp +++ b/rct_ros_tools/src/command_line_cal.cpp @@ -176,7 +176,7 @@ struct DataCollection { cv::Mat image = images[i]; auto msg = poses[i]; - Eigen::Affine3d pose; + Eigen::Isometry3d pose; tf::transformMsgToEigen(msg.transform, pose); data.images.push_back(image); diff --git a/rct_ros_tools/src/data_loader/data_set.cpp b/rct_ros_tools/src/data_loader/data_set.cpp index f1569114..62032cc9 100644 --- a/rct_ros_tools/src/data_loader/data_set.cpp +++ b/rct_ros_tools/src/data_loader/data_set.cpp @@ -42,7 +42,7 @@ static rct_ros_tools::ExtrinsicDataSet parse(const YAML::Node& root, const std:: const auto img_path = root[i]["image"].as(); const auto pose_path = root[i]["pose"].as(); cv::Mat image = readImageOpenCV(combine(root_path, img_path)); - Eigen::Affine3d p; + Eigen::Isometry3d p; rct_ros_tools::loadPose(combine(root_path, pose_path), p); data.images.push_back(image); @@ -68,7 +68,7 @@ boost::optional rct_ros_tools::parseFromFile(co } } -void writePose(const std::string& path, const Eigen::Affine3d& pose) +void writePose(const std::string& path, const Eigen::Isometry3d& pose) { YAML::Node root; root["x"] = pose.translation().x(); diff --git a/rct_ros_tools/src/data_loader/parameter_loaders.cpp b/rct_ros_tools/src/data_loader/parameter_loaders.cpp index 1221d8ba..8b8e93f3 100644 --- a/rct_ros_tools/src/data_loader/parameter_loaders.cpp +++ b/rct_ros_tools/src/data_loader/parameter_loaders.cpp @@ -61,12 +61,12 @@ bool rct_ros_tools::loadIntrinsics(const ros::NodeHandle& nh, const std::string& } bool rct_ros_tools::loadPose(const ros::NodeHandle& nh, const std::string& key, - Eigen::Affine3d& pose) + Eigen::Isometry3d& pose) { XmlRpc::XmlRpcValue xml; if (!nh.getParam(key, xml)) return false; - pose = Eigen::Affine3d::Identity(); + pose = Eigen::Isometry3d::Identity(); double x, y, z, qx, qy, qz, qw; if (!read(xml, "x", x)) return false; if (!read(xml, "y", y)) return false; @@ -82,7 +82,7 @@ bool rct_ros_tools::loadPose(const ros::NodeHandle& nh, const std::string& key, return true; } -bool rct_ros_tools::loadPose(const std::string& path, Eigen::Affine3d& pose) +bool rct_ros_tools::loadPose(const std::string& path, Eigen::Isometry3d& pose) { YAML::Node n = YAML::LoadFile(path); Eigen::Vector3d position; @@ -97,7 +97,7 @@ bool rct_ros_tools::loadPose(const std::string& path, Eigen::Affine3d& pose) qy = n["qy"].as(); qz = n["qz"].as(); - pose = Eigen::Affine3d::Identity(); + pose = Eigen::Isometry3d::Identity(); pose.translation() = position; pose.linear() = Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix(); return true;