C++ Vector3f::norm方法代码示例

    /**
     * @brief Computes the trackball's rotation, using stored initial and final position vectors.
     */
    void computeRotationAngle (void)
    {
        //Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.

        if(initialPosition.norm() > 0) {
            initialPosition.normalize();
        }
        if(finalPosition.norm() > 0) {
            finalPosition.normalize();
        }

        //cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;

        Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);

        if(rotationAxis.norm() != 0) {
            rotationAxis.normalize();
        }

        float dot = initialPosition.dot(finalPosition);

        float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.

        Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));

        quaternion = q * quaternion;
        quaternion.normalize();
    }

float RobotNodeSet::getMaximumExtension()
{
	float result = 0;
	Eigen::Matrix4f t;
	Eigen::Vector3f v;
	if (kinematicRoot && robotNodes.size()>0)
	{
		t = kinematicRoot->getTransformationTo(robotNodes[0]);
		v = MathTools::getTranslation(t);
		result += v.norm();
	}

	for (size_t i=0;i<this->robotNodes.size()-1;i++)
	{
		t = robotNodes[i]->getTransformationTo(robotNodes[i+1]);
		v = MathTools::getTranslation(t);
		result += v.norm();
	}

	if (tcp && robotNodes.size()>0)
	{
		t = tcp->getTransformationTo(robotNodes[robotNodes.size()-1]);
		v = MathTools::getTranslation(t);
		result += v.norm();
	}
	return result;
}

float robotDepth()
{
	// Two interesting points
	float alpha1 = (x + (sx-height)/2)/(float)height*2*tan(VFOV/2.);
	float alpha2 = (x - (sx+height)/2)/(float)height*2*tan(VFOV/2.);
	float beta = (y - width/2)/(float)width*2*tan(HFOV/2.);

	// Vectors
	Eigen::Vector3f v1(1,-beta,-alpha1);
	Eigen::Vector3f v2(1,-beta,-alpha2);

	// Normalization
	v1 = v1/v1.norm();
	v2 = v2/v2.norm();

	// Center
	Eigen::Vector3f c = (v1+v2)/2.;
	float c_norm = c.norm();

	// Projection
	Eigen::MatrixXf proj_mat = c.transpose()*v1;
	float proj = proj_mat(0,0);

	// Orthogonal part in v1
	Eigen::Vector3f orth = v1 - proj/c_norm*c;

	// Norm
	float orth_norm = orth.norm();

	// Approximate depth
	float d = H/2.*proj/orth_norm;
	return d;
}

bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
                     Eigen::Affine3f& transf)
{
  // uz: versor pointing toward the destination
  Eigen::Vector3f uz = target - rayo;
  if (std::abs(uz.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
    return false;
  }
  uz.normalize();
  //std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
  // ux: versor pointing toward the ground
  Eigen::Vector3f ux = down - down.dot(uz) * uz;  
  if (std::abs(ux.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
    return false;
  }
  ux.normalize();
  //std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
  Eigen::Vector3f uy = uz.cross(ux);
  //std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
  Eigen::Matrix3f rot;
  rot << ux.x(), uy.x(), uz.x(),
         ux.y(), uy.y(), uz.y(),
         ux.z(), uy.z(), uz.z();
  transf.setIdentity();
  transf.translate(rayo);
  transf.rotate(rot);
  //std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
  return true;
}

// Rotate the Vector 'normal_to_rotate' into 'base_normal'
// Returns a rotation matrix which can be used for the transformation
// The matrix also includes an empty translation vector
Eigen::Matrix< float, 4, 4 > rotateAroundCrossProductOfNormals(
    Eigen::Vector3f base_normal,
    Eigen::Vector3f normal_to_rotate)
{
    // M
    // Eigen::Vector3f plane_normal(dest.x, dest.y, dest.z);

    // Compute the necessary rotation to align a face of the object with the camera's
    // imaginary image plane
    // N
    // Eigen::Vector3f camera_normal;
    // camera_normal(0)=0;
    // camera_normal(1)=0;
    // camera_normal(2)=1;
    // Eigen::Vector3f camera_normal_normalized = camera_normal.normalized();
    float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );

    Eigen::Vector3f axis;
    Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
    // axis = plane_normal.cross(camera_normal) / (plane_normal.cross(camera_normal)).normalize();
    firstAxis.normalize();
    axis=firstAxis;
    float c = costheta;
    std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
    float s = sqrt(1-c*c);
    float CO = 1-c;

    float x = axis(0);
    float y = axis(1);
    float z = axis(2);
    
    Eigen::Matrix< float, 4, 4 > rotationBox;
    rotationBox(0,0) = x*x*CO+c;
    rotationBox(1,0) = y*x*CO+z*s;
    rotationBox(2,0) = z*x*CO-y*s;

    rotationBox(0,1) = x*y*CO-z*s;
    rotationBox(1,1) = y*y*CO+c;
    rotationBox(2,1) = z*y*CO+x*s;

    rotationBox(0,2) = x*z*CO+y*s;
    rotationBox(1,2) = y*z*CO-x*s;
    rotationBox(2,2) = z*z*CO+c;
   // Translation vector
    rotationBox(0,3) = 0;
    rotationBox(1,3) = 0;
    rotationBox(2,3) = 0;

    // The rest of the 4x4 matrix
    rotationBox(3,0) = 0;
    rotationBox(3,1) = 0;
    rotationBox(3,2) = 0;
    rotationBox(3,3) = 1;

    return rotationBox;
}

Eigen::Matrix< float, 4, 4 > IAMethod::rotateAroundCrossProductOfNormals(
    Eigen::Vector3f base_normal,
    Eigen::Vector3f normal_to_rotate,
    bool store_transformation)
{
  normal_to_rotate *= -1; // The model is standing upside down, rotate the normal by 180 DEG
  float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );

  Eigen::Vector3f axis;
  Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
  firstAxis.normalize();
  axis=firstAxis;
  float c = costheta;
  std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
  float s = sqrt(1-c*c);
  float CO = 1-c;

  float x = axis(0);
  float y = axis(1);
  float z = axis(2);

  Eigen::Matrix< float, 4, 4 > rotationBox;
  rotationBox(0,0) = x*x*CO+c;
  rotationBox(1,0) = y*x*CO+z*s;
  rotationBox(2,0) = z*x*CO-y*s;

  rotationBox(0,1) = x*y*CO-z*s;
  rotationBox(1,1) = y*y*CO+c;
  rotationBox(2,1) = z*y*CO+x*s;

  rotationBox(0,2) = x*z*CO+y*s;
  rotationBox(1,2) = y*z*CO-x*s;
  rotationBox(2,2) = z*z*CO+c;
  // Translation vector
  rotationBox(0,3) = 0;
  rotationBox(1,3) = 0;
  rotationBox(2,3) = 0;

  // The rest of the 4x4 matrix
  rotationBox(3,0) = 0;
  rotationBox(3,1) = 0;
  rotationBox(3,2) = 0;
  rotationBox(3,3) = 1;

  if(store_transformation)
    rotations_.push_back(rotationBox);

  return rotationBox;
}

bool Pcl_grabing::checkForHand()
{
    update_kinect_points();
    int npts = transformed_pclKinect_clr_ptr_->points.size();
    Eigen::Vector3f pt;
    Eigen::Vector3f dist;
    double distance = 1;
    vector<int> index;
    index.clear();
    for (int i = 0; i < npts; i++) 
    {
        pt = transformed_pclKinect_clr_ptr_->points[i].getVector3fMap();
        dist = pt - TableCentroid;
        dist[2]=0;
        distance = dist.norm();
        if(distance < TableRadius)
        {
            if(pt[2]>(TableHeight+HandMinHeight))
            {
                index.push_back(i);
            }
        }
    }
    int n_hand_points = index.size();
    if(n_hand_points<20)
    {
        ROS_INFO("there is no hand.");
        return 0;
    }
    ROS_INFO("got hand.");
    return true;
}

size_t closest_point_index_rayOMP(const pcl::PointCloud<PointT>& cloud,
                                  const Eigen::Vector3f& direction_pre,
                                  const Eigen::Vector3f& line_pt,
                                  double& distance_to_ray) {
    Eigen::Vector3f direction = direction_pre / direction_pre.norm();
    PointT point;
    std::vector<double> distances(cloud.points.size(), 10);  // Initialize to value 10
// Generate a vector of distances
    #pragma omp parallel for
    for (size_t index = 0; index < cloud.points.size(); index++) {
        point = cloud.points[index];

        Eigen::Vector3f cloud_pt(point.x, point.y, point.z);
        Eigen::Vector3f difference = (line_pt - cloud_pt);
        // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation
        double distance = (difference - (difference.dot(direction) * direction)).norm();
        distances[index] = distance;
    }

    double min_distance = std::numeric_limits<double>::infinity();
    size_t min_index = 0;
    // Find index of maximum (TODO: Figure out how to OMP this)
    for (size_t index = 0; index < cloud.points.size(); index++) {
        const double distance = distances[index];
        if (distance < min_distance) {
            min_distance = distance;
            min_index = index;
        }
    }

    distance_to_ray = min_distance;

    return (min_index);
}

float cPointLight::Shadow(const cScene &scene, const Eigen::Vector3f &p, Eigen::Vector3f &l) const
{
    l = pos - p;

    float Dist = l.norm();
    float attenuation = DistScale / Dist;

    l /= Dist;

    cRay ray(l, p); // shadow ray
    sMaterial Texture;
    // check all occluding objects
    attenuation = attenuation * attenuation;// distance attenuation is prop. to squared dist.

    for (std::pair<const aWorldObject *, float> occlude = scene.intersect(ray, Dist);
            occlude.first && occlude.second < Dist;
            occlude = scene.intersect(ray, Dist))
    {
        ray.orig = ray.point(occlude.second);
        Texture = occlude.first->getMaterialAt(ray.orig);

        if (Texture.mKTransp < cRayTracer::mThreshold) // object is opaque
            return 0;

        if ((attenuation *= Texture.mKTransp) < cRayTracer::mThreshold)
            return 0;

        Dist -= occlude.second;
    }

    return attenuation;
}

bool DihedralConstraint::initConstraint(SimulationModel &model, const unsigned int particle1, const unsigned int particle2,
										const unsigned int particle3, const unsigned int particle4)
{
	m_bodies[0] = particle1;
	m_bodies[1] = particle2;
	m_bodies[2] = particle3;
	m_bodies[3] = particle4;
	ParticleData &pd = model.getParticles();

	const Eigen::Vector3f &p0 = pd.getPosition0(particle1);
	const Eigen::Vector3f &p1 = pd.getPosition0(particle2);
	const Eigen::Vector3f &p2 = pd.getPosition0(particle3);
	const Eigen::Vector3f &p3 = pd.getPosition0(particle4);

	Eigen::Vector3f e = p3 - p2;
	float  elen = e.norm();
	if (elen < 1e-6f)
		return false;

	float invElen = 1.0f / elen;

	Eigen::Vector3f n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.squaredNorm();
	Eigen::Vector3f n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.squaredNorm();

	n1.normalize();
	n2.normalize();
	float dot = n1.dot(n2);

	if (dot < -1.0f) dot = -1.0f;
	if (dot > 1.0f) dot = 1.0f;

	m_restAngle = acosf(dot);

	return true;
}

bool SimoxRobotViewer::showVector( const std::string &vecName, const Eigen::Vector3f &pos, const Eigen::Vector3f &ori, float scaling )
{
	removeVector(vecName);

	lock();
	SoSeparator* sep = new SoSeparator();
	sep->addChild(CoinVisualizationFactory::CreateVertexVisualization(pos,5,0,1,0,0));
	if (ori.norm()>1e-9 && scaling>0)
	{
		SoTranslation* t = new SoTranslation();
		//cout << "ori:\n" << ori << endl;
		t->translation.setValue(pos[0],pos[1],pos[2]);
		sep->addChild(t);
		SoSeparator* sepA = CoinVisualizationFactory::CreateArrow(ori,50.0f*scaling,2.0f*scaling,VirtualRobot::VisualizationFactory::Color::Blue());
		sep->addChild(sepA);
	}

	SoSeparator* sText = CoinVisualizationFactory::CreateText(vecName);
	if (sText)
		sep->addChild(sText);
	vectors[vecName] = sep;

	// add visu
	sceneSep->addChild(sep);

	unlock();
	return true;
}

bool SlamEngine::IsTransformationBigEnough()
{
	if (!using_optimizer)
	{
		return false;
	}
	if (accumulated_frame_count >= Config::instance()->get<int>("max_keyframe_interval"))
	{
		return true;
	}

	Eigen::Vector3f t = TranslationFromMatrix4f(accumulated_transformation);
	float tnorm = t.norm();
	Eigen::Vector3f e = EulerAngleFromQuaternion(QuaternionFromMatrix4f(accumulated_transformation));
	e *= 180.0 / M_PI;
	float max_angle = std::max(fabs(e(0)), std::max(fabs(e(1)), fabs(e(2))));

	cout << ", " << tnorm << ", " << max_angle;

	if (tnorm > Config::instance()->get<float>("min_translation_meter")
		|| max_angle > Config::instance()->get<float>("min_rotation_degree"))
	{
		return true;
	}

	return false;
}

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
    const Eigen::Vector3f& origin,
    const Eigen::Vector3f& end,
    AlignedPointTVector &voxel_center_list,
    float precision)
{
  Eigen::Vector3f direction = end - origin;
  float norm = direction.norm ();
  direction.normalize ();

  const float step_size = static_cast<const float> (resolution_) * precision;
  // Ensure we get at least one step for the first voxel.
  const int nsteps = std::max (1, static_cast<int> (norm / step_size));

  OctreeKey prev_key;

  bool bkeyDefined = false;

  // Walk along the line segment with small steps.
  for (int i = 0; i < nsteps; ++i)
  {
    Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));

    PointT octree_p;
    octree_p.x = p.x ();
    octree_p.y = p.y ();
    octree_p.z = p.z ();

    OctreeKey key;
    this->genOctreeKeyforPoint (octree_p, key);

    // Not a new key, still the same voxel.
    if ((key == prev_key) && (bkeyDefined) )
      continue;

    prev_key = key;
    bkeyDefined = true;

    PointT center;
    genLeafNodeCenterFromOctreeKey (key, center);
    voxel_center_list.push_back (center);
  }

  OctreeKey end_key;
  PointT end_p;
  end_p.x = end.x ();
  end_p.y = end.y ();
  end_p.z = end.z ();
  this->genOctreeKeyforPoint (end_p, end_key);
  if (!(end_key == prev_key))
  {
    PointT center;
    genLeafNodeCenterFromOctreeKey (end_key, center);
    voxel_center_list.push_back (center);
  }

  return (static_cast<int> (voxel_center_list.size ()));
}

void display(void)
{
   glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    for(float i = 0; i<width-1; i+=1){
        for(float j = 0; j<width-1; j+=1){
    glBegin(GL_POLYGON);
        Mass m1 = *masses[i*width+j];
        Mass m2 = *masses[i*width+j+1];
        Mass m3 = *masses[(i+1)*width+j+1];
        Mass m4 = *masses[(i+1)*width+j];
        Eigen::Vector3f m11;
        Eigen::Vector3f m12;
        Eigen::Vector3f m13;
        Eigen::Vector3f m14;
        Eigen::Vector3f n;
        m11 << m1.position.x(),m1.position.y(),m1.position.z();
        m12 << m2.position.x(),m2.position.y(),m2.position.z();
        m14 << m4.position.x(),m4.position.y(),m4.position.z();
        n = (m11-m12).cross(m11-m14);
        n = n/n.norm();
        glNormal3f(n.x(),n.y(),n.z());
        glVertex3f(m1.position.x(),m1.position.y(),m1.position.z());
        glNormal3f(n.x(),n.y(),n.z());
        glVertex3f(m2.position.x(),m2.position.y(),m2.position.z());
        glNormal3f(n.x(),n.y(),n.z());
        glVertex3f(m4.position.x(),m4.position.y(),m4.position.z());
    glEnd();
    glBegin(GL_POLYGON);
        m12 << m2.position.x(),m2.position.y(),m2.position.z();
        m13 << m3.position.x(),m3.position.y(),m3.position.z();
        m14 << m4.position.x(),m4.position.y(),m4.position.z();
        n = -(m12-m13).cross(m12-m14);
        n = n/n.norm();
        glNormal3f(n.x(),n.y(),n.z());
        glVertex3f(m2.position.x(),m2.position.y(),m2.position.z());
        glNormal3f(n.x(),n.y(),n.z());
        glVertex3f(m3.position.x(),m3.position.y(),m3.position.z());
        glNormal3f(n.x(),n.y(),n.z());
        glVertex3f(m4.position.x(),m4.position.y(),m4.position.z());
    glEnd();
        }
    }

   glFlush ();
}

/** Update this line.
 * @param linfo new info to consume
 * This also updates moving averages for all fields.
 */
void TrackedLineInfo::update(LineInfo &linfo)
{
  if (visibility_history <= 0)
    visibility_history = 1;
  else
    visibility_history += 1;

  this->raw = linfo;
  fawkes::tf::Stamped<fawkes::tf::Point> bp_new(
	  fawkes::tf::Point(
	      linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
	  ), fawkes::Time(0,0), input_frame_id);
  try {
    transformer->transform_point(tracking_frame_id, bp_new, this->base_point_odom);
  } catch (fawkes::tf::TransformException &e) {
    logger->log_warn(plugin_name.c_str(), "Can't transform to %s. Attempting to track in %s.",
										 tracking_frame_id.c_str(), input_frame_id.c_str());
    this->base_point_odom = bp_new;
  }
  this->history.push_back(linfo);

  Eigen::Vector3f base_point_sum(0,0,0), end_point_1_sum(0,0,0),
	  end_point_2_sum(0,0,0), line_direction_sum(0,0,0), point_on_line_sum(0,0,0);
  float length_sum(0);
  for (LineInfo &l : this->history) {
	base_point_sum += l.base_point;
	end_point_1_sum += l.end_point_1;
	end_point_2_sum += l.end_point_2;
	line_direction_sum += l.line_direction;
	point_on_line_sum += l.point_on_line;
	length_sum += l.length;
  }

  size_t sz = this->history.size();
  this->smooth.base_point = base_point_sum / sz;
  this->smooth.cloud = linfo.cloud;
  this->smooth.end_point_1 = end_point_1_sum / sz;
  this->smooth.end_point_2 = end_point_2_sum / sz;
  this->smooth.length = length_sum / sz;
  this->smooth.line_direction = line_direction_sum / sz;
  this->smooth.point_on_line = point_on_line_sum / sz;

  Eigen::Vector3f x_axis(1,0,0);

  Eigen::Vector3f ld_unit = this->smooth.line_direction / this->smooth.line_direction.norm();
  Eigen::Vector3f pol_invert = Eigen::Vector3f(0,0,0) - this->smooth.point_on_line;
  Eigen::Vector3f P = this->smooth.point_on_line + pol_invert.dot(ld_unit) * ld_unit;
  this->smooth.bearing = std::acos(x_axis.dot(P) / P.norm());
  // we also want to encode the direction of the angle
  if (P[1] < 0)
    this->smooth.bearing = std::abs(this->smooth.bearing) * -1.;

  Eigen::Vector3f l_diff = raw.end_point_2 - raw.end_point_1;
  Eigen::Vector3f l_ctr = raw.end_point_1 + l_diff / 2.;
  this->bearing_center = std::acos(x_axis.dot(l_ctr) / l_ctr.norm());
  if (l_ctr[1] < 0)
    this->bearing_center = std::abs(this->bearing_center) * -1.;
}