C++ Vector4d::dot方法代码示例

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;

}