std::vector<float> SimpleRayCaster::castRay(Eigen::Vector2f &p) { std::vector<float> zvalues; /* distort the point with the lens parameters */ Eigen::Vector2f pDist = distortPoint(p); /* convert 2d coordinate (0.0-1.0) to proper image coordinate */ Eigen::Vector3f aP(pDist[0]*_res_x, pDist[1]*_res_y, 1.0); /* project with inverse camera calibration matrix * to 3d vector in camera space */ cv::Mat ptMat = (cv::Mat_<float>(3,1) << aP[0], aP[1], aP[2]); cv::Mat vec3d = _inverseCamMat * ptMat; Eigen::Vector3f pvec(vec3d.at<float>(0,0), vec3d.at<float>(1,0), vec3d.at<float>(2,0)); std::vector<Eigen::Vector3f> isecs = intersectRay(pvec); for (auto isec : isecs) { zvalues.push_back(isec.norm()); } return zvalues; }
//\fn void ExtrinsicParam::getCameraPointFrom3d(Eigen::Vector3d realP, double &x, double &y, double &z); ///\brief This function computes the coordinates of a 3D point from the real landmark to the one of the camera. ///\param realP Value of the 3D point in the real landmark. ///\param x Value of the 3D point in the camera landmark and on the x axis. ///\param y Value of the 3D point in the camera landmark and on the y axis. ///\param z Value of the distance between the camera and the 3D point (the scale). void ExtrinsicParam::getCameraPointFrom3d(Eigen::Vector3d realP, double &x, double &y, double &z) { Eigen::Vector4d real; real << realP, 1; double fx = K(0,0), fy = K(1,1), cx = K(0,2), cy = K(1,2); Eigen::Vector3d imgP; Eigen::MatrixXd Rt; Rt.resize(3,4); Rt << rotation,translation; imgP.noalias() = K*Rt*real; z = imgP(2); imgP.noalias() = imgP/imgP(2); Eigen::Vector3d temp = imgP; temp(0) = (temp(0)-cx)/fx; temp(1) = (temp(1)-cy)/fy; temp.noalias() = distortPoint(temp); x = temp(0); y = temp(1); }