Update library to use Isometry3d instead of Affine3d (#31)

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<cv::Mat>& image_set = maybe_data_set->images;
-  const std::vector<Eigen::Affine3d>& wrist_poses = maybe_data_set->tool_poses;
+  const std::vector<Eigen::Isometry3d>& 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();
 

rct_examples/src/tools/camera_on_wrist_extrinsic.cpp

@@ -15,14 +15,14 @@
 #include <opencv2/imgproc.hpp>
 #include <rct_optimizations/ceres_math_utilities.h>
 
-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<cv::Point2d> 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();
 

rct_examples/src/tools/multi_static_camera_extrinsic.cpp

@@ -16,15 +16,15 @@
 #include <rct_optimizations/ceres_math_utilities.h>
 #include <rct_optimizations/experimental/multi_camera_pnp.h>
 
-static void reproject(const Eigen::Affine3d& base_to_target,
-                      const std::vector<Eigen::Affine3d>& base_to_camera,
+static void reproject(const Eigen::Isometry3d& base_to_target,
+                      const std::vector<Eigen::Isometry3d>& base_to_camera,
                       const std::vector<rct_optimizations::CameraIntrinsics>& intr,
                       const rct_image_tools::ModifiedCircleGridTarget& target,
                       const cv::Mat& image,
                       const std::vector<rct_optimizations::CorrespondenceSet>& 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<cv::Point2d> 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<rct_optimizations::CorrespondenceSet> corr_set;
-    std::vector<Eigen::Affine3d> base_to_camera;
-    Eigen::Affine3d base_to_wrist;
+    std::vector<Eigen::Isometry3d> base_to_camera;
+    Eigen::Isometry3d base_to_wrist;
     std::vector<rct_optimizations::CameraIntrinsics> intr;
     cv::Mat image;
 

rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp

@@ -16,15 +16,15 @@
 #include <rct_optimizations/ceres_math_utilities.h>
 #include <rct_optimizations/experimental/multi_camera_pnp.h>
 
-static void reproject(const Eigen::Affine3d& base_to_target,
-                      const std::vector<Eigen::Affine3d>& base_to_camera,
+static void reproject(const Eigen::Isometry3d& base_to_target,
+                      const std::vector<Eigen::Isometry3d>& base_to_camera,
                       const std::vector<rct_optimizations::CameraIntrinsics>& intr,
                       const rct_image_tools::ModifiedCircleGridTarget& target,
                       const cv::Mat& image,
                       const std::vector<rct_optimizations::CorrespondenceSet>& 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<cv::Point2d> 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<rct_optimizations::CorrespondenceSet> corr_set;
-    std::vector<Eigen::Affine3d> base_to_camera;
-    Eigen::Affine3d base_to_target;
+    std::vector<Eigen::Isometry3d> base_to_camera;
+    Eigen::Isometry3d base_to_target;
     std::vector<rct_optimizations::CameraIntrinsics> intr;
 
     corr_set.reserve(num_of_cameras);

rct_examples/src/tools/solve_multi_camera_pnp.cpp

@@ -17,15 +17,15 @@
 #include <rct_ros_tools/data_set.h>
 #include <rct_ros_tools/print_utils.h>
 
-static void reproject(const Eigen::Affine3d& base_to_target,
-                      const std::vector<Eigen::Affine3d>& base_to_camera,
+static void reproject(const Eigen::Isometry3d& base_to_target,
+                      const std::vector<Eigen::Isometry3d>& base_to_camera,
                       const std::vector<rct_optimizations::CameraIntrinsics>& intr,
                       const rct_image_tools::ModifiedCircleGridTarget& target,
                       const cv::Mat& image,
                       const std::vector<rct_optimizations::CorrespondenceSet>& 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<cv::Point2d> 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<Eigen::Affine3d> base_to_camera;
+  std::vector<Eigen::Isometry3d> base_to_camera;
   std::vector<rct_optimizations::CameraIntrinsics> intr;
   std::vector<std::string> data_path;
   std::string target_path;

rct_examples/src/tools/solve_pnp.cpp

@@ -14,7 +14,7 @@
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/print_utils.h>
 
-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<Eigen::Vector2d>& 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<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(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<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(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;

rct_examples/src/tools/static_camera_extrinsic.cpp

@@ -15,12 +15,12 @@
 #include <rct_optimizations/ceres_math_utilities.h>
 #include <rct_optimizations/experimental/pnp.h>
 
-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<cv::Point2d> 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();
 

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<cv::Point2d> getReprojections(const Eigen::Affine3d &camera_to_target,
+std::vector<cv::Point2d> getReprojections(const Eigen::Isometry3d &camera_to_target,
                                           const rct_optimizations::CameraIntrinsics &intr,
                                           const std::vector<Eigen::Vector3d> &target_points)
 {

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);
 
 }
 

rct_optimizations/include/rct_optimizations/experimental/camera_intrinsic.h

@@ -13,7 +13,7 @@ struct IntrinsicEstimationProblem
   CameraIntrinsics intrinsics_guess;
 
   bool use_extrinsic_guesses;
-  std::vector<Eigen::Affine3d> extrinsic_guesses;
+  std::vector<Eigen::Isometry3d> extrinsic_guesses;
 };
 
 struct IntrinsicEstimationResult
@@ -25,7 +25,7 @@ struct IntrinsicEstimationResult
   CameraIntrinsics intrinsics;
   std::array<double, 5> distortions;
 
-  std::vector<Eigen::Affine3d> target_transforms;
+  std::vector<Eigen::Isometry3d> target_transforms;
 };
 
 IntrinsicEstimationResult optimize(const IntrinsicEstimationProblem& params);

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<Eigen::Affine3d> base_to_camera;
+  std::vector<Eigen::Isometry3d> 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<CorrespondenceSet> 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);

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);

rct_optimizations/include/rct_optimizations/extrinsic_3d_camera_on_wrist.h

@@ -26,11 +26,11 @@ namespace rct_optimizations
 
 struct Extrinsic3DCameraOnWristProblem
 {
-  std::vector<Eigen::Affine3d> wrist_poses;
+  std::vector<Eigen::Isometry3d> wrist_poses;
   std::vector<Correspondence3DSet> 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);