Esempio n. 1
0
    /**
     * @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();
    }
Esempio n. 2
0
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;
}
Esempio n. 4
0
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;
}
Esempio n. 5
0
// 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;
}
Esempio n. 6
0
bool Vis::collidingWith(const Object& object) const
{
	Eigen::Vector3f const swimDirection(goalPos - this->pos),
	                      objectDirection(object.pos - this->pos);
	float const swimDistance   = swimDirection.norm(),
	            objectDistance = objectDirection.norm();

	if (objectDistance < (this->collisionRadius * this->scale + object.collisionRadius * object.scale))
	{
		/*
		 * We're using this theorem from the dot product, u and v are
		 * vectors, T is the smallest angle between those vectors.
		 *
		 * u · v = |u| |v| cos T
		 *
		 * Thus:
		 *           u · v
		 * cos T = ---------
		 *          |u| |v|
		 *
		 * Additionally, given that T is always the smallest angle,
		 * thus T is always <= π. Also cos x / dx for 0 <= x <= π is
		 * always <= 0. Thus for S,T in [0,π] S < T, S > T and S = T
		 * imply cos S < cos T, cos S > T and cos S = T respectively.
		 *
		 * Thus while we cannot use cos T as T, we can use it to
		 * compare the angle T with another angle S. Hence we don't
		 * need to find T by obtaining arccos cos T, which we would be
		 * quite computationally expensive.
		 */
		float const cos_angle = swimDirection.dot(objectDirection)
		                      / (swimDistance * objectDistance);

		/*
		 * Only act on our collision if *this* is the object crashing
		 * right into it, i.e. our angle with that object is less than
		 * 90˚. If our angle is greater than 90˚ then we are in fact
		 * already traveling away from the colliding object. For angles
		 * x from 0˚ to 90˚ cos(x) ranges from 1 to 0, i.e. remains
		 * positive.
		 */
		if (cos_angle >= 0.f)
		{
			collided = collisionVisualLength;
			return true;
		}
	}

	return false;
}
Esempio n. 7
0
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;
}
Esempio n. 8
0
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;
}
Esempio n. 9
0
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);
}
Esempio n. 10
0
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;
}
Esempio n. 12
0
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;
}
Esempio n. 13
0
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;
}
Esempio n. 14
0
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 ()));
}
Esempio n. 15
0
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 ();
}
Esempio n. 16
0
/** 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.;
}
Esempio n. 17
0
bool Camera::collides(const Entity &e) {
//    return false;
    float cam_rad = collisionRadius();

    if (e.useBoundingBox()) {
        //      return true;
        BoundingBox bb = e.getBoundingBox();
        bb.box.min += e.getPosition();
        bb.box.max += e.getPosition();
        Eigen::Vector3f our_pos = -translations;
        if (bb.contains(our_pos)) {
            return true;
        }

        Eigen::Vector3f displacement = (bb.box.center() - our_pos);
        float distance = displacement.norm();
        Eigen::Vector3f direction = displacement.normalized();

        if (bb.contains(direction * cam_rad + our_pos)) {
            return true;
        }

        return false;
        
    }
    else {
        Eigen::Vector3f their_pos = e.getCenterWorld();
        Eigen::Vector3f our_pos = translations;
        their_pos(1) = 0;
        our_pos(1) = 0;
        Eigen::Vector3f delta = -their_pos - our_pos;
        return std::abs(delta.norm()) < cam_rad + e.getRadius();
    }
    
    
    /*
    std::cout << "object pos" << std::endl;
    std::cout << their_pos << std::endl;
    std::cout << "cam pos" << std::endl;
    std::cout << our_pos << std::endl;
    std::cout << " dist = " << std::abs(delta.norm()) << std::endl;
    */

    
}
Esempio n. 18
0
inline void 
RangeImagePlanar::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const 
{
  Eigen::Vector3f transformedPoint = to_range_image_system_ * point;
  range = transformedPoint.norm ();
  
  image_x = center_x_ + focal_length_x_*transformedPoint[0]/transformedPoint[2];
  image_y = center_y_ + focal_length_y_*transformedPoint[1]/transformedPoint[2];
}
Esempio n. 19
0
inline void 
RangeImageSpherical::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const
{
  Eigen::Vector3f transformedPoint = to_range_image_system_ * point;
  range = transformedPoint.norm ();
  float angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]),
        angle_y = asinLookUp (transformedPoint[1]/range);
  getImagePointFromAngles (angle_x, angle_y, image_x, image_y);
}
bool CUDACameraTrackingMultiResRGBD::checkRigidTransformation(Eigen::Matrix3f& R, Eigen::Vector3f& t, float angleThres, float distThres) {
	Eigen::AngleAxisf aa(R);

	if (aa.angle() > angleThres || t.norm() > distThres) {
		std::cout << "Tracking lost: angle " << (aa.angle()/M_PI)*180.0f << " translation " << t.norm() << std::endl;
		return false;
	}

	return true;
}
Esempio n. 21
0
void ImuDeadReckon::makeQuaternionFromVector( Eigen::Vector3f& inVec, Eigen::Quaternionf& outQuat )
{
    float phi = inVec.norm();
    Eigen::Vector3f u = inVec / phi; // u is a unit vector

    outQuat.vec() = u * sin( phi / 2.0 );
    outQuat.w()   =     cos( phi / 2.0 );


    //outQuat = Eigen::Quaternionf( 1.0, 0., 0., 0. );
}
double angular_difference(geometry_msgs::Quaternion c,geometry_msgs::Quaternion d){
	Eigen::Vector4f dv;
	dv[0] = d.w; dv[1] = d.x; dv[2] = d.y; dv[3] = d.z;
	Eigen::Matrix<float, 3,4> inv;
	inv(0,0) = -c.x; inv(0,1) = c.w; inv(0,2) = -c.z; inv(0,3) = c.y;
	inv(1,0) = -c.y; inv(1,1) = c.z; inv(1,2) = c.w;	inv(1,3) = -c.x;
	inv(2,0) = -c.z; inv(2,1) = -c.y;inv(2,2) = c.x;  inv(2,3) = c.w;
	
	Eigen::Vector3f m = inv * dv * -2.0;
	return m.norm();
}
Esempio n. 23
0
Eigen::Vector3f PhotoCamera::unproject(const Eigen::Vector2f &pixel)
{
    Eigen::Vector3f pixelH;
    pixelH << pixel(0), pixel(1), 1;
    Eigen::MatrixXf pseudoInverse = getPseudoInverse();
    Eigen::Vector4f XH = pseudoInverse*pixelH;
    Eigen::Vector3f p = XH.hnormalized();
    Eigen::Vector3f ray = p - cameraCenter.hnormalized();
    ray /= ray.norm();

    return ray;
}
Esempio n. 24
0
 void RobotNodePrismatic::setVisuScaleFactor(Eigen::Vector3f& scaleFactor)
 {
     if (scaleFactor.norm() == 0.0f)
     {
         visuScaling = false;
     }
     else
     {
         visuScaling = true;
         visuScaleFactor = scaleFactor;
     }
 }
Esempio n. 25
0
void Render::Arrow::draw(Render::Style style) const
{
  Eigen::Vector3f point;
  Render::Cylinder cylinder;
  Render::Cone cone;

  point = terminus_ - origin_;
  point = point * (point.norm() - 0.3f) / point.norm();
  point = point + origin_;

  cylinder.setVertex1(origin_);
  cylinder.setVertex2(point);
  cylinder.setRadius(radius_);
  cylinder.setMaterial(material_);
  cylinder.draw(style, 24);

  cone.setVertex1(point);
  cone.setVertex2(terminus_);
  cone.setRadius(radius_ * 2);
  cone.setMaterial(material_);
  cone.draw(style);
}
Esempio n. 26
0
void CoaxVisionControl::StateCallback(const coax_msgs::CoaxState::ConstPtr & msg) {
    static int initTime = 200;
    static int initCounter = 0;

    static Eigen::Vector3f init_acc(0,0,0);
    static Eigen::Vector3f init_gyr(0,0,0);

    battery_voltage = msg->battery;
    coax_nav_mode = msg->mode.navigation;

    //	rpy << msg->roll, msg->pitch, msg->yaw;
    //	std::cout << "RPY: \n" << rpy << std::endl;
    accel << msg->accel[0], msg->accel[1], msg->accel[2];
    gyro << msg->gyro[0], msg->gyro[1], msg->gyro[2];
    rpyt_rc << msg->rcChannel[4], msg->rcChannel[6], msg->rcChannel[2], msg->rcChannel[0];
    rpyt_rc_trim << msg->rcChannel[5], msg->rcChannel[7], msg->rcChannel[3], msg->rcChannel[1];
    range_al = msg->zfiltered;

    global_z = msg->zfiltered;
    if (FIRST_STATE) {
        last_state_time = ros::Time::now().toSec();
        FIRST_STATE = false;
        ROS_INFO("First Time Stamp: %f",last_state_time);
        return;
    }

    if ((battery_voltage < 10.50) && !LOW_POWER_DETECTED) {
        ROS_INFO("Battery Low!!! (%fV) Landing initialized",battery_voltage);
        LOW_POWER_DETECTED = true;
    }

    if (initCounter < initTime) {
        init_acc += accel;
        init_gyr += gyro;
        initCounter++;
    }
    else if (initCounter == initTime) {
        init_acc /= initTime;
        gravity = init_acc.norm();
        setGravity(gravity);
        ROS_INFO("IMU Calibration Done! Gravity: %f", gravity);
        initCounter++;

        setInit(msg->header.stamp);
    }
    else {
        processUpdate(accel, gyro, msg->header.stamp);
        measureUpdate(range_al);
    }
    return;
}
	void CGLUtil::viewerGL()
	{
		glMatrixMode(GL_MODELVIEW);
		// load the matrix to set camera pose
		glLoadMatrixf(_eimModelViewGL.data());
		
		//rotation
		Eigen::Matrix3f eimRotation;
		if( btl::utility::BTL_GL == _eConvention ){
			eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX());                         // 3. rotate horizontally
		}//mouse x-movement is the rotation around y-axis
		else if( btl::utility::BTL_CV == _eConvention )	{
			eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), -Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX());                         // 3. rotate horizontally
		}
		//translation
		/*_dZoom = _dZoom < 0.1? 0.1: _dZoom;
		_dZoom = _dZoom > 10? 10: _dZoom;*/

		//get direction N pointing from camera center to the object centroid
		Eigen::Affine3f M; M = _eimModelViewGL;
		Eigen::Vector3f T = M.translation();
		Eigen::Matrix3f R = M.linear();
		Eigen::Vector3f C = -R.transpose()*T;//camera centre
		Eigen::Vector3f N = _eivCentroid - C;//from camera centre to object centroid
		N = N/N.norm();//normalization

		Eigen::Affine3f _eimManipulate; _eimManipulate.setIdentity();
		_eimManipulate.translate( N*float(_dZoom) );//(N*(1-_dZoom));  //use camera movement toward object for zoom in/out effects
		_eimManipulate.translate(_eivCentroid);  // 5. translate back to the original camera pose
		//_eimManipulate.scale(s);				 // 4. zoom in/out, never use scale to simulate camera movement, it affects z-buffer capturing. use translation instead
		_eimManipulate.rotate(eimRotation);		 // 2. rotate vertically // 3. rotate horizontally
		_eimManipulate.translate(-_eivCentroid); // 1. translate the camera center to align with object centroid*/
		glMultMatrixf(_eimManipulate.data());

		/*	
		lTranslated( _aCentroid[0], _aCentroid[1], _aCentroid[2] ); // 5. translate back to the original camera pose
		_dZoom = _dZoom < 0.1? 0.1: _dZoom;
		_dZoom = _dZoom > 10? 10: _dZoom;
		glScaled( _dZoom, _dZoom, _dZoom );                      //  4. zoom in/out, 
		if( btl::utility::BTL_GL == _eConvention )
		glRotated ( _dXAngle, 0, 1 ,0 );                         // 3. rotate horizontally
		else if( btl::utility::BTL_CV == _eConvention )			//mouse x-movement is the rotation around y-axis
		glRotated ( _dXAngle, 0,-1 ,0 ); 
		glRotated ( _dYAngle, 1, 0 ,0 );                             // 2. rotate vertically
		glTranslated(-_aCentroid[0],-_aCentroid[1],-_aCentroid[2] ); // 1. translate the camera center to align with object centroid
		*/

		// light position in 3d
		glLightfv(GL_LIGHT0, GL_POSITION, _aLight);
	}
Esempio n. 28
0
Eigen::Vector3f VisuilizeProcessor::VectorToEulerAngles(Eigen::Vector3f v)
{
    Eigen::Vector3f normed = v / v.norm();
    float x = normed(0);
    float y = normed(1);
    float z = normed(2);

    Eigen::Vector3f result;
    result(0) = std::atan2(z, x) - M_PI / 2; // yaw
    result(1) = -std::atan2(y, z); // tilt
    result(2) = std::atan(y / x); // roll

    return result * 180 / M_PI;

}
/*void CameraPoseFinderICP::findCorresSet(unsigned level,const Mat44& cur_transform,const Mat44& last_transform_inv)
{
	 cudaProjectionMapFindCorrs(level,cur_transform,last_transform_inv,
			 _camera_params_pyramid[level],
			AppParams::instance()->_icp_params.fDistThres,
			AppParams::instance()->_icp_params.fNormSinThres);

}*/
bool CameraPoseFinderICP::vector6ToTransformMatrix(const Eigen::Matrix<float, 6, 1, 0, 6, 1>& x,  Eigen::Matrix4f& output)
{
	Eigen::Matrix3f R = Eigen::AngleAxisf(x[0], Eigen::Vector3f::UnitX()).toRotationMatrix()
		*Eigen::AngleAxisf(x[1], Eigen::Vector3f::UnitY()).toRotationMatrix()
		*Eigen::AngleAxisf(x[2], Eigen::Vector3f::UnitZ()).toRotationMatrix();
	Eigen::Vector3f t = x.segment(3, 3);
	Eigen::AngleAxisf aa(R);
	float angle = aa.angle();
	float d = t.norm();
	if (angle > AppParams::instance()->_icp_params.fAngleShake|| d> AppParams::instance()->_icp_params.fDistShake)
	{
		return false;
	}
	output.block(0, 0, 3, 3) = R;
	output.block(0, 3, 3, 1) = t;
	return true;
}
Esempio n. 30
0
// This formula comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
void
psmove_alignment_compute_objective_vector(
	const Eigen::Quaternionf &q, const Eigen::Vector3f &d, const Eigen::Vector3f &s,
	Eigen::Matrix<float, 3, 1> &out_f, float *out_squared_error)
{
	// Computing f(q;d, s) = (q^-1 * d * q) - s
	Eigen::Vector3f d_rotated = psmove_vector3f_clockwise_rotate(q, d);
	Eigen::Vector3f f = d_rotated - s;

	out_f(0, 0) = f.x();
	out_f(1, 0) = f.y();
	out_f(2, 0) = f.z();

	if (out_squared_error)
	{
		*out_squared_error = f.norm();
	}
}