void ObjectModelLine::projectPoints (const std::vector<int> &inliers, const Eigen::VectorXd &model_coefficients,
PointCloud3D* projectedPointCloud){
assert (model_coefficients.size () == 6);
// Obtain the line point and direction
Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4d pt ((*inputPointCloud->getPointCloud())[inliers[i]].getX(), (*inputPointCloud->getPointCloud())[inliers[i]].getY(),
(*inputPointCloud->getPointCloud())[inliers[i]].getZ(), 0);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
double k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
Eigen::Vector4d pp = line_pt + k * line_dir;
// Calculate the projection of the point on the line (pointProj = A + k * B)
// std::vector<Point3D> *projectedPoints = projectedPointCloud->getPointCloud();
(*projectedPointCloud->getPointCloud())[i].setX(pp[0]);
(*projectedPointCloud->getPointCloud())[i].setY(pp[1]);
(*projectedPointCloud->getPointCloud())[i].setZ(pp[2]);
}
}
double
ObjectModelCylinder::pointToLineDistance (const Eigen::Vector4d &pt, const Eigen::Vector4d &line_pt, const Eigen::Vector4d &line_dir)
{
// Calculate the distance from the point to the line
// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
Eigen::Vector4d r, p_t;
r = line_pt + line_dir;
p_t = r - pt;
#ifdef EIGEN3
Eigen::Vector3d c = p_t.head<3> ().cross (line_dir.head<3> ());
#else
Eigen::Vector3d c = p_t.start<3> ().cross (line_dir.start<3> ());
#endif
return (sqrt (c.dot (c) / line_dir.dot (line_dir)));
}
void ObjectModelCylinder::selectWithinDistance (const Eigen::VectorXd &model_coefficients, double threshold,
std::vector<int> &inliers){
assert (model_coefficients.size () == 7);
int nr_p = 0;
inliers.resize (this->inputPointCloud->getSize());
Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
double ptdotdir = line_pt.dot (line_dir);
double dirdotdir = 1.0 / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < this->inputPointCloud->getSize(); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
Eigen::Vector4d pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[i].getX(),
(*inputPointCloud->getPointCloud())[i].getY(),
(*inputPointCloud->getPointCloud())[i].getZ(), 0);
Eigen::Vector4d n = Eigen::Vector4d (this->normals->getNormals()->data()[i].getX(),
this->normals->getNormals()->data()[i].getY(),
this->normals->getNormals()->data()[i].getZ(), 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4d pt_proj = line_pt + k * line_dir;
Eigen::Vector4d dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = fmin (d_normal, M_PI - d_normal);
if (fabs (this->normalDistanceWeight * d_normal + (1 - this->normalDistanceWeight) * d_euclid) < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = i;
nr_p++;
}
}
inliers.resize (nr_p);
}
bool FrustumCulling::Contains(const Eigen::Vector3d& point) const
{
Eigen::Vector4d pointHomo = toHomo( point );
return
(pointHomo.dot (leftPlane_) <= 0) &&
(pointHomo.dot (rightPlane_) <= 0) &&
(pointHomo.dot (topPlane_) <= 0) &&
(pointHomo.dot (bottomPlane_) <= 0) &&
(pointHomo.dot (nearPlane_) <= 0) &&
// el plano lejano no se tiene en cuenta para filtrar puntos (se podria omitir su computo)
(pointHomo.dot (farPlane_) <= 0);
}
void ObjectModelCylinder::getDistancesToModel (const Eigen::VectorXd &model_coefficients, std::vector<double> &distances){
assert (model_coefficients.size () == 7);
distances.resize (this->inputPointCloud->getSize());
Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
double ptdotdir = line_pt.dot (line_dir);
double dirdotdir = 1.0 / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < this->inputPointCloud->getSize(); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
// Todo to be revised
Eigen::Vector4d pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[i].getX(),
(*inputPointCloud->getPointCloud())[i].getY(), (*inputPointCloud->getPointCloud())[i].getZ(), 0);
Eigen::Vector4d n = Eigen::Vector4d (this->normals->getNormals()->data()[i].getX(),
this->normals->getNormals()->data()[i].getY(),
this->normals->getNormals()->data()[i].getZ(), 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4d pt_proj = line_pt + k * line_dir;
Eigen::Vector4d dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = fmin (d_normal, M_PI - d_normal);
distances[i] = fabs (this->normalDistanceWeight * d_normal + (1 - this->normalDistanceWeight)
* d_euclid);
}
}
Eigen::Vector4d getColor(const Ray &ray, unsigned int recursionLevel, unsigned int transDepth,
std::array<int, MAX_DEPTH + 1> &objStack) {
BOOST_ASSERT_MSG(std::abs(1 - ray.dir.norm()) < EPSILON, "Got ray with non-unit direction");
const intersection_t isect = getIntersection(ray);
int objId;
if ((objId = isect.objId) < 0) return Eigen::Vector4d::Zero();
auto &objects = scene.getObjects();
auto &lights = scene.getLights();
Eigen::Vector4d I = Eigen::Vector4d::Zero();
Eigen::Vector4d pointOfIntersection = ray.origin + isect.dist * ray.dir;
Eigen::Vector4d N = objects[objId]->getUnitNormal(pointOfIntersection);
Eigen::Vector4d V = -1 * ray.dir;
auto mat = scene.getMaterial(objects[objId]->matId);
for (unsigned int i = 0 ; i < lights.size(); ++i) {
if (lights[i]->isAmbient()) {
I += mat.Ka * lights[i]->getAmountOfLight(pointOfIntersection).cwiseProduct(mat.rgb);
continue;
}
Eigen::Vector4d L = lights[i]->getVectorToLight(pointOfIntersection);
Ray rayToLight(pointOfIntersection, L, objId);
bool lightVisible = true;
if (lights[i]->getShadowOn()) {
double distToBlockingObject = getIntersection(rayToLight).dist;
double distToLight = (pointOfIntersection - lights[i]->getPosition()).norm();
lightVisible = distToBlockingObject <= EPSILON ||
distToBlockingObject >= distToLight;
}
if (lightVisible) {
/* Diffuse Reflection */
Eigen::Vector4d Il = lights[i]->getAmountOfLight(pointOfIntersection);
// Amount of light visible on surface determined by angle to light source
double lambertCos = L.dot(N);
if (lambertCos > 0) {
I += mat.Kd * lambertCos * mat.rgb.cwiseProduct(Il);
} else {
continue;
}
/* Specular Reflection */
Eigen::Vector4d reflection = 2 * N.dot(rayToLight.dir) * N - rayToLight.dir;
double specCoeff = reflection.dot(V);
if (specCoeff > 0) {
specCoeff = std::max(0.0, pow(specCoeff, mat.ns));
I += specCoeff * mat.Ks * mat.rgb.cwiseProduct(Il);
}
}
}
if (recursionLevel < MAX_DEPTH) {
// Work out where in material stack we are
int nextTransDepth;
int nextObjId;
if (objStack[transDepth] == objId) {
nextTransDepth = transDepth - 1;
nextObjId = objStack[transDepth - 1];
} else {
nextTransDepth = transDepth + 1;
objStack[nextTransDepth] = objId;
nextObjId = objId;
}
// Compute intensity of reflected ray, if necessary
if (reflect && mat.Kr > 0) {
Ray reflectionRay(pointOfIntersection, ray.dir - (2 * N.dot(ray.dir)) * N, nextObjId);
I += mat.Kr * getColor(reflectionRay, recursionLevel + 1, nextTransDepth, objStack);
}
// Compute intensity of transmission ray, if necessary
if (refract && mat.Kt > 0) {
const double etaIncident = (objStack[transDepth] == ID_AIR) ? 1.0 :
scene.getMaterial(objects[objStack[transDepth]]->matId).Irefract;
double cosThetaI, etaRefract;
if (objStack[transDepth] == objId) { // Exiting a material
cosThetaI = ray.dir.dot(N);
etaRefract = (objStack[transDepth - 1] == ID_AIR) ? 1.0 :
scene.getMaterial(objects[objStack[transDepth - 1]]->matId).Irefract;
} else {
cosThetaI = V.dot(N);
etaRefract = scene.getMaterial(objects[objId]->matId).Irefract;
}
const double n = etaIncident/etaRefract;
const double cosThetaR = sqrt(1 - (n * n) * (1 - cosThetaI * cosThetaI));
Ray T(pointOfIntersection, (n * ray.dir - (cosThetaR - n * cosThetaI) * N).normalized(), nextObjId);
I += mat.Kt * getColor(T, recursionLevel + 1, nextTransDepth, objStack);
}
}
return I;
}