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