separate stereo calib

raphaelchang · Sep 26, 2019 · 7b33038 · 7b33038
1 parent 9a03e54
commit 7b33038
Show file tree

Hide file tree

Showing 15 changed files with 194 additions and 59 deletions.
diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch
@@ -3,6 +3,7 @@
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
     <arg name="camera_model" default="double_sphere" />
     <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="stereo_camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -25,6 +26,7 @@
         <rosparam subst_value="true">
             camera_model: '$(arg camera_model)'
             camera_parameters: $(arg camera_params)
+            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             tracker_window_size: 128

diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch
@@ -3,6 +3,7 @@
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
     <arg name="camera_model" default="perspective" />
     <arg name="camera_params" default="{fx: 721.5377, fy: 721.5377, cx: 609.5593, cy: 172.854}" />
+    <arg name="stereo_camera_params" default="{fx: 721.5377, fy: 721.5377, cx: 609.5593, cy: 172.854}" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -25,6 +26,7 @@
         <rosparam subst_value="true">
             camera_model: '$(arg camera_model)'
             camera_parameters: $(arg camera_params)
+            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             tracker_window_size: 128

diff --git a/launch/slam_eval_t265.launch b/launch/slam_eval_t265.launch
@@ -3,6 +3,7 @@
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
     <arg name="camera_model" default="double_sphere" />
     <arg name="camera_params" default="{fx: 287.405, fy: 287.092, cx: 418.432, cy: 399.257, chi: -0.0000588, alpha: 0.675621}" />
+    <arg name="stereo_camera_params" default="{fx: 287.405, fy: 287.092, cx: 418.432, cy: 399.257, chi: -0.0000588, alpha: 0.675621}" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -25,6 +26,7 @@
         <rosparam subst_value="true">
             camera_model: '$(arg camera_model)'
             camera_parameters: $(arg camera_params)
+            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             tracker_window_size: 128

diff --git a/launch/slam_eval_tum.launch b/launch/slam_eval_tum.launch
@@ -0,0 +1,60 @@
+<launch>
+    <arg name="bag_file" default="" />
+    <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
+    <arg name="camera_model" default="double_sphere" />
+    <arg name="camera_params" default="{fx: 313.533725, fy: 313.51138991, cx: 510.37735, cy: 514.419593, chi: -0.18065, alpha: 0.59186286}" />
+    <arg name="stereo_camera_params" default="{fx: 313.4386621, fy: 313.359903, cx: 505.72413717, cy: 510.5791992, chi: -0.17829397, alpha: 0.591774861}" />
+    <arg name="rate" default="1" />
+    <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
+        <param name="bag_file" value="$(arg bag_file)" />
+        <param name="results_file" value="$(arg results_file)" />
+        <param name="image_topic" value="/cam0/image_raw" />
+        <param name="stereo_image_topic" value="/cam1/image_raw" />
+        <param name="depth_image_topic" value="" />
+        <param name="pose_topic" value="/vrpn_client/raw_transform" />
+        <param name="tracked_image_topic" value="/omni_slam/tracked" />
+        <param name="odometry_estimate_topic" value="/omni_slam/odometry" />
+        <param name="odometry_optimized_topic" value="/omni_slam/odometry_optimized" />
+        <param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
+        <param name="path_estimate_topic" value="/omni_slam/odometry_path" />
+        <param name="path_optimized_topic" value="/omni_slam/odometry_path_optimized" />
+        <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
+        <param name="point_cloud_topic" value="/omni_slam/reconstructed" />
+        <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
+        <param name="output_frame" value="map" />
+        <param name="rate" value="$(arg rate)" />
+        <rosparam subst_value="true">
+            camera_model: '$(arg camera_model)'
+            camera_parameters: $(arg camera_params)
+            stereo_camera_parameters: $(arg stereo_camera_params)
+            detector_type: 'GFTT'
+            detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
+            tracker_window_size: 128
+            tracker_num_scales: 4
+            tracker_checker_epipolar_threshold: 0.008
+            tracker_checker_iterations: 1000
+            tracker_delta_pixel_error_threshold: 0.0
+            tracker_error_threshold: 1000000.0
+            min_features_per_region: 100
+            max_features_per_region: 1000
+            pnp_inlier_threshold: 3.0
+            pnp_iterations: 3000
+            max_reprojection_error: 5.0
+            min_triangulation_angle: 3.0
+            bundle_adjustment_max_iterations: 1000
+            bundle_adjustment_loss_coefficient: 0.05
+            bundle_adjustment_logging: true
+            bundle_adjustment_num_threads: 20
+            local_bundle_adjustment_window: 0
+            local_bundle_adjustment_interval: 0
+            stereo_matcher_window_size: 256
+            stereo_matcher_num_scales: 5
+            stereo_matcher_error_threshold: 20
+            stereo_matcher_epipolar_threshold: 0.008
+            stereo_tf_t: [0.1010976, 0.001878, 0.00117]
+            stereo_tf_r: [-0.0237542, 0.0002876, -0.0004555, 0.9997177]
+        </rosparam>
+    </node>
+</launch>
+
+
diff --git a/src/camera/double_sphere.h b/src/camera/double_sphere.h
@@ -122,7 +122,8 @@ class DoubleSphere : public CameraModel<T>
         T r2;
         if (alpha_ > 0.5)
         {
-            r2 = std::min(mx * mx, 1. / (2. * alpha_ - 1.));
+            //r2 = std::min(mx * mx, 1. / (2. * alpha_ - 1.));
+            r2 = 1. / (2. * alpha_ - 1.);
         }
         else
         {

diff --git a/src/data/feature.cc b/src/data/feature.cc
@@ -7,16 +7,18 @@ namespace omni_slam
 namespace data
 {
 
-Feature::Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor)
+Feature::Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor, bool stereo)
     : frame_(frame),
     kpt_(kpt),
-    descriptor_(descriptor)
+    descriptor_(descriptor),
+    stereo_(stereo)
 {
 }
 
-Feature::Feature(Frame &frame, cv::KeyPoint kpt)
+Feature::Feature(Frame &frame, cv::KeyPoint kpt, bool stereo)
     : frame_(frame),
-    kpt_(kpt)
+    kpt_(kpt),
+    stereo_(stereo)
 {
 }
 
@@ -40,7 +42,14 @@ Vector3d Feature::GetBearing() const
     Vector3d cameraFramePt;
     Vector2d pixelPt;
     pixelPt << kpt_.pt.x + 0.5, kpt_.pt.y + 0.5;
-    frame_.GetCameraModel().UnprojectToBearing(pixelPt, cameraFramePt);
+    if (stereo_)
+    {
+        frame_.GetStereoCameraModel().UnprojectToBearing(pixelPt, cameraFramePt);
+    }
+    else
+    {
+        frame_.GetCameraModel().UnprojectToBearing(pixelPt, cameraFramePt);
+    }
     return util::TFUtil::CameraFrameToWorldFrame(cameraFramePt);
 }
 

diff --git a/src/data/feature.h b/src/data/feature.h
@@ -15,8 +15,8 @@ namespace data
 class Feature
 {
 public:
-    Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor);
-    Feature(Frame &frame, cv::KeyPoint kpt);
+    Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor, bool stereo = false);
+    Feature(Frame &frame, cv::KeyPoint kpt, bool stereo = false);
 
     const Frame& GetFrame() const;
     const cv::KeyPoint& GetKeypoint() const;
@@ -34,6 +34,7 @@ class Feature
     cv::Mat descriptor_;
     Vector3d worldPoint_;
     Vector3d worldPointEstimate_;
+    bool stereo_;
 
     bool worldPointCached_{false};
     bool worldPointEstimateCached_{false};

diff --git a/src/data/frame.cc b/src/data/frame.cc
@@ -8,7 +8,7 @@ namespace data
 
 int Frame::lastFrameId_ = 0;
 
-Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
+Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model)
     : id_(lastFrameId_++),
     image_(image.clone()),
     stereoImage_(stereo_image.clone()),
@@ -17,21 +17,23 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix
     invPose_(util::TFUtil::InversePoseMatrix(pose)),
     stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)),
     timeSec_(time),
-    cameraModel_(camera_model)
+    cameraModel_(camera_model),
+    stereoCameraModel_(&stereo_camera_model)
 {
     hasPose_ = true;
     hasDepth_ = true;
     hasStereo_ = true;
     poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
-Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
+Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model)
     : id_(lastFrameId_++),
     image_(image.clone()),
     stereoImage_(stereo_image.clone()),
     stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)),
     timeSec_(time),
-    cameraModel_(camera_model)
+    cameraModel_(camera_model),
+    stereoCameraModel_(&stereo_camera_model)
 {
     hasPose_ = false;
     hasDepth_ = false;
@@ -52,14 +54,15 @@ Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraMo
     pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
-Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
+Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model)
     : id_(lastFrameId_++),
     image_(image.clone()),
     stereoImage_(stereo_image.clone()),
     depthImage_(depth_image.clone()),
     stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)),
     timeSec_(time),
-    cameraModel_(camera_model)
+    cameraModel_(camera_model),
+    stereoCameraModel_(&stereo_camera_model)
 {
     hasPose_ = false;
     hasDepth_ = true;
@@ -81,23 +84,24 @@ Frame::Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::C
     poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
-Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
+Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model)
     : id_(lastFrameId_++),
     image_(image.clone()),
     stereoImage_(stereo_image.clone()),
     pose_(pose),
     invPose_(util::TFUtil::InversePoseMatrix(pose)),
     stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)),
     timeSec_(time),
-    cameraModel_(camera_model)
+    cameraModel_(camera_model),
+    stereoCameraModel_(&stereo_camera_model)
 {
     hasPose_ = true;
     hasDepth_ = false;
     hasStereo_ = true;
     poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
-Frame::Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
+Frame::Frame(cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model)
     : id_(lastFrameId_++),
     image_(image.clone()),
     depthImage_(depth_image.clone()),
@@ -194,6 +198,11 @@ const camera::CameraModel<>& Frame::GetCameraModel() const
     return cameraModel_;
 }
 
+const camera::CameraModel<>& Frame::GetStereoCameraModel() const
+{
+    return *stereoCameraModel_;
+}
+
 const Matrix<double, 3, 4>& Frame::GetEstimatedPose() const
 {
     return poseEstimate_;

diff --git a/src/data/frame.h b/src/data/frame.h
@@ -16,13 +16,13 @@ namespace data
 class Frame
 {
 public:
-    Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
-    Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
+    Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model);
+    Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model);
     Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
-    Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
+    Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model);
     Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model);
-    Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
-    Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
+    Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model);
+    Frame(cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model);
     Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model);
     Frame(const Frame &other);
 
@@ -33,6 +33,7 @@ class Frame
     const cv::Mat& GetStereoImage();
     const Matrix<double, 3, 4>& GetStereoPose() const;
     const camera::CameraModel<>& GetCameraModel() const;
+    const camera::CameraModel<>& GetStereoCameraModel() const;
     const Matrix<double, 3, 4>& GetEstimatedPose() const;
     const Matrix<double, 3, 4>& GetEstimatedInversePose() const;
     const int GetID() const;
@@ -72,6 +73,7 @@ class Frame
     Matrix<double, 3, 4> invPoseEstimate_;
     double timeSec_;
     camera::CameraModel<> &cameraModel_;
+    camera::CameraModel<> *stereoCameraModel_{nullptr};
 
     std::unordered_set<int> estLandmarkIds_;
 

diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc
@@ -80,21 +80,24 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
 
             const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID());
             Vector2d pixelGnd;
-            if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd))
+            if (frames_.back()->HasPose() && landmark.HasGroundTruth())
             {
-                Vector2d pixel;
-                pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y;
-                double error = (pixel - pixelGnd).norm();
+                if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd))
+                {
+                    Vector2d pixel;
+                    pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y;
+                    double error = (pixel - pixelGnd).norm();
 
-                visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i);
+                    visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i);
 
-                double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5;
-                double yg = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5;
-                double rg = sqrt(xg * xg + yg * yg) / imsize;
-                stats_.radialErrors.emplace_back(vector<double>{rg, error});
-                stats_.frameErrors.emplace_back(vector<double>{(double)landmark.GetNumObservations() - 1, (double)i, rg, error});
+                    double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5;
+                    double yg = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5;
+                    double rg = sqrt(xg * xg + yg * yg) / imsize;
+                    stats_.radialErrors.emplace_back(vector<double>{rg, error});
+                    stats_.frameErrors.emplace_back(vector<double>{(double)landmark.GetNumObservations() - 1, (double)i, rg, error});
+                }
             }
-            else if (!frames_.back()->HasPose())
+            else
             {
                 visualization_.AddTrack(obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, i);
             }

diff --git a/src/optimization/reprojection_error.h b/src/optimization/reprojection_error.h
@@ -45,7 +45,8 @@ class ReprojectionError
         {
             Matrix<T, 2, 1> reprojPoint2;
             Matrix<T, 3, 4> stereoPose = stereoFeature_.GetFrame().GetStereoPose().cast<T>();
-            camera.ProjectToImage(util::TFUtil::TransformPoint(stereoPose, camPt), reprojPoint2);
+            C<T> stereoCamera(feature_.GetFrame().GetStereoCameraModel());
+            stereoCamera.ProjectToImage(util::TFUtil::TransformPoint(stereoPose, camPt), reprojPoint2);
             reproj_error[0] += reprojPoint(0) - T(stereoFeature_.GetKeypoint().pt.x);
             reproj_error[1] += reprojPoint(1) - T(stereoFeature_.GetKeypoint().pt.y);
         }