UndistortDriver::UndistortDriver(std::shared_ptr<CameraDriverInterface> input, const std::shared_ptr<calibu::Rig<double> > rig ) : m_Input(input) { const size_t num_cams = rig->NumCams(); m_vLuts.resize(num_cams); for(size_t ii=0; ii< num_cams; ++ii) { const std::shared_ptr<calibu::CameraInterface<double>> cmod = rig->cameras_[ii]; // Allocate memory for LUTs. m_vLuts[ii] = calibu::LookupTable(cmod->Width(), cmod->Height()); // Setup new camera model // For now, assume no change in scale so return same params with // no distortion. Eigen::Vector2i size_; Eigen::VectorXd params_(static_cast<int>(calibu::LinearCamera<double>::NumParams)); size_ << cmod->Width(), cmod->Height(); params_ << cmod->K()(0,0), cmod->K()(1,1), cmod->K()(0,2), cmod->K()(1,2); std::shared_ptr<calibu::CameraInterface<double>> new_cam(new calibu::LinearCamera<double>(params_, size_)); m_CamModel.push_back(new_cam); calibu::CreateLookupTable(rig->cameras_[ii], new_cam->K().inverse(), m_vLuts[ii]); } }
UndistortDriver::UndistortDriver( std::shared_ptr<CameraDriverInterface> input, const calibu::CameraRig& rig ) : m_Input(input) { const size_t num_cams = rig.cameras.size(); m_vLuts.resize(num_cams); for(size_t ii=0; ii< num_cams; ++ii) { const calibu::CameraModel& cmod = rig.cameras[ii].camera; // Allocate memory for LUTs. m_vLuts[ii] = calibu::LookupTable(cmod.Width(), cmod.Height()); // Setup new camera model // For now, assume no change in scale so return same params with // no distortion. calibu::CameraModelT<calibu::Pinhole> new_cam(cmod.Width(), cmod.Height()); new_cam.Params() << cmod.K()(0,0), cmod.K()(1,1), cmod.K()(0,2), cmod.K()(1,2); m_CamModel.push_back(new_cam); calibu::CreateLookupTable(rig.cameras[ii].camera, new_cam.Kinv(), m_vLuts[ii]); } }
calibu::CameraModelT<Pinhole> CreateScanlineRectifiedLookupAndCameras( const Sophus::SE3d T_rl, const calibu::CameraModelInterface& cam_left, const calibu::CameraModelInterface& cam_right, Sophus::SE3d& T_nr_nl, LookupTable& left_lut, LookupTable& right_lut ) { const Sophus::SO3d R_rl = T_rl.so3(); const Sophus::SO3d R_lr = R_rl.inverse(); const Eigen::Vector3d l_r = T_rl.translation(); const Eigen::Vector3d r_l = - (R_lr * l_r); // Current up vector for each camera (in left FoR) const Eigen::Vector3d lup_l = Eigen::Vector3d(0,-1,0); const Eigen::Vector3d rup_l = R_lr * Eigen::Vector3d(0,-1,0); // Hypothetical fwd vector for each camera, perpendicular to baseline (in left FoR) const Eigen::Vector3d lfwd_l = (lup_l.cross(r_l)).normalized(); const Eigen::Vector3d rfwd_l = (rup_l.cross(r_l)).normalized(); // New fwd is average of left / right hypothetical baselines (also perpendicular to baseline) const Eigen::Vector3d avgfwd_l = lfwd_l + rfwd_l; // Define new basis (in left FoR); const Eigen::Vector3d x_l = r_l.normalized(); const Eigen::Vector3d z_l = avgfwd_l.normalized(); const Eigen::Vector3d y_l = z_l.cross(x_l).normalized(); // New orientation for both left and right cameras (expressed relative to original left) Eigen::Matrix3d Rnl_l; Rnl_l << x_l, y_l, z_l; // By definition, the right camera now lies exactly on the x-axis with the same orientation // as the left camera. T_nr_nl = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(-r_l.norm(),0,0) ); // Work out parameters of new linear camera const Range range_width = //Intersection( MinMaxRotatedCol(cam_left, Rnl_l); // MinMaxRotatedCol(cam_right, Rnl_l) // ); const Range range_height = //Intersection( MinMaxRotatedRow(cam_left, Rnl_l); // MinMaxRotatedRow(cam_right, Rnl_l) // ); // We want to map range width/height to image via K. const double fu = (cam_left.Width()-1) / range_width.Size(); const double fv = (cam_left.Height()-1) / range_height.Size(); const double u0 = -fu * range_width.minr; const double v0 = -fv * range_height.minr; // Setup new camera calibu::CameraModelT<Pinhole> new_cam(cam_left.Width(),cam_left.Height()); new_cam.Params() << fu, fv, u0, v0; // Homographies which should be applied to left and right images to scan-line rectify them const Eigen::Matrix3d Rl_nlKlinv = Rnl_l.transpose() * new_cam.Kinv(); const Eigen::Matrix3d Rr_nrKlinv = R_lr.inverse().matrix() * Rnl_l.transpose() * new_cam.Kinv(); CreateLookupTable(cam_left, Rl_nlKlinv, left_lut ); CreateLookupTable(cam_right, Rr_nrKlinv, right_lut ); return new_cam; }