void Task::createPointcloudFromMLS(PCLPointCloudPtr pointcloud, envire::MultiLevelSurfaceGrid* mls_grid)
{
    pointcloud->clear();
    
    float vertical_distance = (mls_grid->getScaleX() + mls_grid->getScaleY()) * 0.5;
    if(vertical_distance <= 0.0)
        vertical_distance = 0.1;
    
    // create pointcloud from mls
    for(size_t x=0;x<mls_grid->getCellSizeX();x++)
    {
        for(size_t y=0;y<mls_grid->getCellSizeY();y++)
        {
            for( envire::MLSGrid::iterator cit = mls_grid->beginCell(x,y); cit != mls_grid->endCell(); cit++ )
            {
                envire::MLSGrid::SurfacePatch p( *cit );
                
                Eigen::Vector3d cellPosWorld = mls_grid->fromGrid(x, y, mls_grid->getEnvironment()->getRootNode());
                pcl::PointXYZ point;
                point.x = cellPosWorld.x();
                point.y = cellPosWorld.y();
                point.z = cellPosWorld.z();
                if(p.isHorizontal())
                {
                    point.z = cellPosWorld.z() + p.mean;
                    pointcloud->push_back(point);
                }
                else if(p.isVertical())
                {
                    float min_z = (float)p.getMinZ(0);
                    float max_z = (float)p.getMaxZ(0);
                    for(float z = min_z; z <= max_z; z += vertical_distance)
                    {
                        point.z = cellPosWorld.z() + z;
                        pointcloud->push_back(point);
                    }
                }
            }
        }
    }
}
//! Compute gravitational acceleration due to J2.
Eigen::Vector3d computeGravitationalAccelerationDueToJ2(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameterOfBodyExertingAcceleration,
        const double equatorialRadiusOfBodyExertingAcceleration,
        const double j2CoefficientOfGravityField,
        const Eigen::Vector3d& positionOfBodyExertingAcceleration )
{
    // Set constant values reused for optimal computation of acceleration components.
    const double distanceBetweenBodies = ( positionOfBodySubjectToAcceleration
                                           - positionOfBodyExertingAcceleration ).norm( );

    const double preMultiplier = -gravitationalParameterOfBodyExertingAcceleration
            / std::pow( distanceBetweenBodies, 4.0 ) * 1.5 * j2CoefficientOfGravityField
            * equatorialRadiusOfBodyExertingAcceleration
            * equatorialRadiusOfBodyExertingAcceleration;

    const double scaledZCoordinate = ( positionOfBodySubjectToAcceleration.z( )
                                       - positionOfBodyExertingAcceleration.z( ) )
            / distanceBetweenBodies;

    const double scaledZCoordinateSquared = scaledZCoordinate * scaledZCoordinate;

    const double factorForXAndYDirections = ( 1.0 - 5.0 * scaledZCoordinateSquared )
            / distanceBetweenBodies;

    // Compute components of acceleration due to J2-effect.
    Eigen::Vector3d gravitationalAccelerationDueToJ2 = Eigen::Vector3d::Constant( preMultiplier );

    gravitationalAccelerationDueToJ2( basic_astrodynamics::xCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.x( )
                 - positionOfBodyExertingAcceleration.x( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ2( basic_astrodynamics::yCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.y( )
                 - positionOfBodyExertingAcceleration.y( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ2( basic_astrodynamics::zCartesianPositionIndex )
            *= ( 3.0 - 5.0 * scaledZCoordinateSquared ) * scaledZCoordinate;

    return gravitationalAccelerationDueToJ2;
}
double
ClosingBoundary::getLineDistance (const Eigen::Vector3d &P0, const Eigen::Vector3d &u, const Eigen::Vector3d &Q0,
                                  const Eigen::Vector3d &v, Eigen::Vector3d &P, Eigen::Vector3d &Q)
{
  Eigen::Vector3d w0 = P0 - Q0;

  double a = u.dot (u);
  double b = u.dot (v);
  double c = v.dot (v);
  double d = u.dot (w0);
  double e = v.dot (w0);

  double s = (b * e - c * d) / (a * c - b * b);
  double t = (a * e - b * d) / (a * c - b * b);

  P = P0 + u * s;
  Q = Q0 + v * t;

  Eigen::Vector3d wc = P - Q;
  return wc.norm ();
}
Beispiel #4
0
//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

  // Y-axis
  const Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();

  // X-axis
  Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
  const double mag = yAxis.dot(pelvisXAxis);
  pelvisXAxis -= mag * yAxis;
  const Eigen::Vector3d xAxis = pelvisXAxis.normalized();

  // Z-axis
  const Eigen::Vector3d zAxis = xAxis.cross(yAxis);

  T.translation() = getCOM();

  T.linear().col(0) = xAxis;
  T.linear().col(1) = yAxis;
  T.linear().col(2) = zAxis;

  return T;
}
	void vision_position_estimate(uint64_t usec,
			Eigen::Vector3d &position,
			Eigen::Vector3d &rpy)
	{
		mavlink::common::msg::VISION_POSITION_ESTIMATE vp{};

		vp.usec = usec;

		// [[[cog:
		// for f in "xyz":
		//     cog.outl("vp.%s = position.%s();" % (f, f))
		// for a, b in zip("xyz", ('roll', 'pitch', 'yaw')):
		//     cog.outl("vp.%s = rpy.%s();" % (b, a))
		// ]]]
		vp.x = position.x();
		vp.y = position.y();
		vp.z = position.z();
		vp.roll = rpy.x();
		vp.pitch = rpy.y();
		vp.yaw = rpy.z();
		// [[[end]]] (checksum: 2048daf411780847e77f08fe5a0b9dd3)

		UAS_FCU(m_uas)->send_message_ignore_drop(vp);
	}
  Line::Line(const Eigen::Vector3d& direction, const Eigen::Vector3d& origin)
    : direction_ (direction.normalized()), origin_(origin)
  {

  }
 Plane::Plane(Eigen::Vector3d normal, Eigen::Vector3d p) :
   normal_(normal), d_(- normal.dot(p))
 {
   
 }
    visualization_msgs::Marker rviz_arrow(const Eigen::Vector3d & arrow, const Eigen::Vector3d & arrow_origin, int id, std::string name_space )
    {
        Eigen::Quaternion<double> rotation;
        if(arrow.norm()<0.0001)
        {
            rotation=Eigen::Quaternion<double>(1,0,0,0);
        }
        else
        {
            double rotation_angle=acos(arrow.normalized().dot(Eigen::Vector3d::UnitX()));
            Eigen::Vector3d rotation_axis=arrow.normalized().cross(Eigen::Vector3d::UnitX()).normalized();
            rotation=Eigen::AngleAxisd(-rotation_angle+PI,rotation_axis);
        }

        visualization_msgs::Marker marker;
        marker.header.frame_id = "/base_link";
        marker.header.stamp = ros::Time();
        marker.id = id;
        if(id==0)
        {
            marker.color.r = 0.0;
            marker.color.g = 0.0;
            marker.color.b = 1.0;
            marker.ns = name_space;
        }
        else
        {
            marker.color.r = 1.0;
            marker.color.g = 0.0;
            marker.color.b = 0.0;
            marker.ns = name_space;
        }
        marker.type = visualization_msgs::Marker::ARROW;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.position.x = arrow_origin.x();
        marker.pose.position.y = arrow_origin.y();
        marker.pose.position.z = arrow_origin.z();
        marker.pose.orientation.x = rotation.x();
        marker.pose.orientation.y = rotation.y();
        marker.pose.orientation.z = rotation.z();
        marker.pose.orientation.w = rotation.w();
        //std::cout <<"position:" <<marker.pose.position << std::endl;
        //std::cout <<"orientation:" <<marker.pose.orientation << std::endl;
        //marker.pose.orientation.x = 0;
        //marker.pose.orientation.y = 0;
        //marker.pose.orientation.z = 0;
        //marker.pose.orientation.w = 1;
        if(arrow.norm()<0.0001)
        {
            marker.scale.x = 0.001;
            marker.scale.y = 0.001;
            marker.scale.z = 0.001;
        }
        else
        {
            marker.scale.x = arrow.norm();
            marker.scale.y = 0.1;
            marker.scale.z = 0.1;
        }
        marker.color.a = 1.0;


        return marker;
    }
	virtual void operate() {
		tmp_theta_pos = this->feedbackjpInput.getValue();
		ThetaInput << tmp_theta_pos[0], tmp_theta_pos[1], tmp_theta_pos[2], tmp_theta_pos[3];
		M_tmp = this->M.getValue();
		Md_tmp = this->Md.getValue();
		tmp_theta_vel = this->feedbackjvInput.getValue();
		ThetadotInput << tmp_theta_vel[0], tmp_theta_vel[1], tmp_theta_vel[2], tmp_theta_vel[3];
		C_tmp = this->C.getValue();
		Cd_tmp = this->Cd.getValue();

		// Reference position, velocity and accelerations
		qd[0] = this->referencejpInput.getValue()[0];
		qd[1] = this->referencejpInput.getValue()[1];
		qd[2] = this->referencejpInput.getValue()[2];
		qd[3] = this->referencejpInput.getValue()[3];

		qdd[0] = this->referencejvInput.getValue()[0];
		qdd[1] = this->referencejvInput.getValue()[1];
		qdd[2] = this->referencejvInput.getValue()[2];
		qdd[3] = this->referencejvInput.getValue()[3];

		qddd[0] = this->referencejaInput.getValue()[0];
		qddd[1] = this->referencejaInput.getValue()[1];
		qddd[2] = this->referencejaInput.getValue()[2];
		qddd[3] = this->referencejaInput.getValue()[3];
//
		//Position error
		e = ThetaInput - qd;
		//Velocity error
		ed = ThetadotInput - qdd;
		//Eta
		eta = K / b;
		//The error matrix
		Xtilde.resize(8, 1);
		Xtilde << e, ed;

		Md_tmpinverse = Md_tmp.inverse();
		M_tmpinverse = M_tmp.inverse();


		F = -M_tmpinverse * (C_tmp);
		Fd = -Md_tmpinverse * (Cd_tmp);

		rho = F - Fd;

		// The fbar vector
		for (i = 0; i < 4; i = i + 1) {
			fbar[i] = rho[i] + eta[i] * c[i] * Xtilde[i]
					+ (c[i] + eta[i]) * Xtilde[4 + i] + eta[i] * phi[i];
		}

//

		Ftilde1 << Xtilde[4], -eta[0] * c[0] * Xtilde[0]
				- (c[0] + eta[0]) * Xtilde[4] - eta[0] * phi[0], 0;
		Ftilde2 << Xtilde[5], -eta[1] * c[1] * Xtilde[1]
				- (c[1] + eta[1]) * Xtilde[5] - eta[1] * phi[1], 0;
		Ftilde3 << Xtilde[6], -eta[2] * c[2] * Xtilde[2]
				- (c[2] + eta[2]) * Xtilde[6] - eta[2] * phi[2], 0;
		Ftilde4 << Xtilde[7], -eta[3] * c[3] * Xtilde[3]
				- (c[3] + eta[3]) * Xtilde[7] - eta[3] * phi[3], 0;

		//The L and Lbar vectors

		for (i = 0; i < 4; i = i + 1) {
			L[i] = 0.5
					* (Xtilde[i] * Xtilde[i] + Xtilde[4 + i] * Xtilde[4 + i]);
			Lbar[i] = L[i] + fbar[i] * fbar[i];

		}

		// The gradient matrices
		DVDX1
				<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
						* W1[0], (W1[0] * Xtilde[0] + W1[1] * Xtilde[4]
				+ W1[2] * phi[0]) * W1[1], (W1[0] * Xtilde[0]
				+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * W1[2];
		DVDX2
				<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
						* W2[0], (W2[0] * Xtilde[1] + W2[1] * Xtilde[5]
				+ W2[2] * phi[1]) * W2[1], (W2[0] * Xtilde[1]
				+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * W2[2];
		DVDX3
				<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
						* W3[0], (W3[0] * Xtilde[2] + W3[1] * Xtilde[6]
				+ W3[2] * phi[2]) * W3[1], (W3[0] * Xtilde[2]
				+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * W3[2];
		DVDX4
				<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
						* W4[0], (W4[0] * Xtilde[3] + W4[1] * Xtilde[7]
				+ W4[2] * phi[3]) * W4[1], (W4[0] * Xtilde[3]
				+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * W4[2];

		DVDW1
				<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
						* Xtilde[0] + W1[0], (W1[0] * Xtilde[0]
				+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * Xtilde[4] + W1[1], (W1[0]
				* Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0]) * phi[0]
				+ W1[2];
		DVDW2
				<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
						* Xtilde[1] + W2[0], (W2[0] * Xtilde[1]
				+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * Xtilde[5] + W2[1], (W2[0]
				* Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1]) * phi[1]
				+ W2[2];
		DVDW3
				<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
						* Xtilde[2] + W3[0], (W3[0] * Xtilde[2]
				+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * Xtilde[6] + W3[1], (W3[0]
				* Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2]) * phi[2]
				+ W3[2];
		DVDW4
				<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
						* Xtilde[3] + W4[0], (W4[0] * Xtilde[3]
				+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * Xtilde[7] + W4[1], (W4[0]
				* Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3]) * phi[3]
				+ W4[2];
////
		s1 = DVDW1.transpose();
		s2 = DVDW2.transpose();
		s3 = DVDW3.transpose();
		s4 = DVDW4.transpose();

		Eigen::Matrix3d tmp_Gtilde;
		tmp_Gtilde << Gtilde[0] * Gtilde[0], Gtilde[0] * Gtilde[1], Gtilde[0]
				* Gtilde[2], Gtilde[1] * Gtilde[0], Gtilde[1] * Gtilde[1], Gtilde[1]
				* Gtilde[2], Gtilde[2] * Gtilde[0], Gtilde[2] * Gtilde[1], Gtilde[2]
				* Gtilde[2];

//
		r1 = -Lbar[0] + 0.25 * DVDX1.dot(tmp_Gtilde * DVDX1)
				- DVDX1.dot(Ftilde1);
		r2 = -Lbar[1] + 0.25 * DVDX2.dot(tmp_Gtilde * DVDX2)
				- DVDX2.dot(Ftilde2);
		r3 = -Lbar[2] + 0.25 * DVDX3.dot(tmp_Gtilde * DVDX3)
				- DVDX3.dot(Ftilde3);
		r4 = -Lbar[3] + 0.25 * DVDX4.dot(tmp_Gtilde * DVDX4)
				- DVDX4.dot(Ftilde4);

		W1dot = s1.transpose() / (s1.dot(s1)) * r1;
		W2dot = s2.transpose() / (s2.dot(s2)) * r2;
		W3dot = s3.transpose() / (s3.dot(s3)) * r3;
		W4dot = s4.transpose() / (s4.dot(s4)) * r4;
////
		ueq1 = 0.5
				* (W1[0] * (W1[2] - W1[1]) * Xtilde[0]
						+ W1[1] * (W1[2] - W1[1]) * Xtilde[4]
						+ W1[2] * (W1[2] - W1[1]) * phi[0]);
		ueq2 = 0.5
				* (W2[0] * (W2[2] - W2[1]) * Xtilde[1]
						+ W2[1] * (W2[2] - W2[1]) * Xtilde[5]
						+ W2[2] * (W2[2] - W2[1]) * phi[1]);
		ueq3 = 0.5
				* (W3[0] * (W3[2] - W3[1]) * Xtilde[2]
						+ W3[1] * (W3[2] - W3[1]) * Xtilde[6]
						+ W3[2] * (W3[2] - W3[1]) * phi[2]);
		ueq4 = 0.5
				* (W4[0] * (W4[2] - W4[1]) * Xtilde[3]
						+ W4[1] * (W4[2] - W4[1]) * Xtilde[7]
						+ W4[2] * (W4[2] - W4[1]) * phi[3]);
//
// Final torque computations
		Ueq << ueq1, ueq2, ueq3, ueq4;

		Ud = qddd + Md_tmpinverse * Cd_tmp;

		Tau = M_tmp * (Ud + Ueq); // Final torque to system.
//

		phidot[0] = -fbar[0] - ueq1;
		phidot[1] = -fbar[1] - ueq2;
		phidot[2] = -fbar[2] - ueq3;
		phidot[3] = -fbar[3] - ueq4;

		phi = phi + phidot * del;
		W1 = W1 + W1dot * del;
		W2 = W2 + W2dot * del;
		W3 = W3 + W3dot * del;
		W4 = W4 + W4dot * del;

		torque_tmp[0] = Tau[0];
		torque_tmp[1] = Tau[1];
		torque_tmp[2] = Tau[2];
		torque_tmp[3] = Tau[3];

//		torque_tmp[0] = 0;
//		torque_tmp[1] = 0;
//		torque_tmp[2] = 0;
//		torque_tmp[3] = 0;

		optslidecontrolOutputValue->setData(&torque_tmp);



	}
void ArnoldAnimationPatch::getPositionFromAss(const set<Device*>& devices)
{
#ifdef USE_ARNOLD
  // by default in y-up coordinate systems, the light faces -z to start with.
  // The procedure here will be to grab the translation component of the arnold matrix
  // (which is actually just the position of the light in 3-d space) and set the
  // look at point to origin - (lookAtDir.normalized()) and the distance to 1.

  for (auto d : devices) {
    // bit of a hack, but don't touch quad light positions, they seem a bit odd
    if (d->getType() == "quad_light")
      continue;

    string nodeName = d->getMetadata("Arnold Node Name");
    AtNode* light = AiNodeLookUpByName(nodeName.c_str());
    
    if (light != nullptr) {
      // Get the matrix
      AtMatrix m;
      AiNodeGetMatrix(light, "matrix", m);

      // Get the light location
      Eigen::Vector3d origin(m[3][0], m[3][1], m[3][2]);

      // Get the rotation matrix
      Eigen::Matrix3d rot;
      rot(0, 0) = m[0][0];
      rot(0, 1) = m[0][1];
      rot(0, 2) = m[0][2];
      rot(1, 0) = m[1][0];
      rot(1, 1) = m[1][1];
      rot(1, 2) = m[1][2];
      rot(2, 0) = m[2][0];
      rot(2, 1) = m[2][1];
      rot(2, 2) = m[2][2];

      // rotate -z to get direction
      Eigen::Vector3d z(0, 0, -1);

      Eigen::Vector3d dir = z.transpose() * rot;
      Eigen::Vector3d look = origin + dir.normalized();

      // calculate spherical coords
      Eigen::Vector3d relPos = origin - look;
      double polar = acos(relPos(1) / relPos.norm()) * (180.0 / M_PI);
      double azimuth = atan2(relPos(2), relPos(0)) * (180 / M_PI);

      // Set params, and lock them to the values found in the file.
      // If the params don't exist, just create them
      if (!d->paramExists("distance"))
        d->setParam("distance", new LumiverseFloat());
      if (!d->paramExists("polar"))
        d->setParam("polar", new LumiverseOrientation());
      if (!d->paramExists("azimuth"))
        d->setParam("azimuth", new LumiverseOrientation());
      if (!d->paramExists("lookAtX"))
        d->setParam("lookAtX", new LumiverseFloat());
      if (!d->paramExists("lookAtY"))
        d->setParam("lookAtY", new LumiverseFloat());
      if (!d->paramExists("lookAtZ"))
        d->setParam("lookAtZ", new LumiverseFloat());

      d->getParam<LumiverseFloat>("distance")->setVals(1, 1, 1, 1);
      d->getParam<LumiverseOrientation>("polar")->setVals((float)polar, (float)polar, (float)polar, (float)polar);
      d->getParam<LumiverseOrientation>("azimuth")->setVals((float)azimuth, (float)azimuth, (float)azimuth, (float)azimuth);
      d->getParam<LumiverseFloat>("lookAtX")->setVals((float)look(0), (float)look(0), (float)look(0), (float)look(0));
      d->getParam<LumiverseFloat>("lookAtY")->setVals((float)look(1), (float)look(1), (float)look(1), (float)look(1));
      d->getParam<LumiverseFloat>("lookAtZ")->setVals((float)look(2), (float)look(2), (float)look(2), (float)look(2));
    }
  }
#endif
}
void Manipulator::computeJacobian(int32_t idx, Eigen::MatrixXd& J)
{
  J = Eigen::MatrixXd::Zero(6, dof_);

  if(idx < dof_) // Not required to consider end-effector
  {
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }
  }
  else // Required to consider the offset of end-effector
  {
    --idx;
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }

    Eigen::MatrixXd J_Pne = Eigen::MatrixXd::Identity(6, 6);
    Eigen::Vector3d Pne;
    if(C_abs_.size() - 1 - 1 >= 0.0)
    {
      Pne = T_abs_[T_abs_.size() - 1].block(0, 3, 3, 1) - C_abs_[C_abs_.size() - 1 - 1].block(0, 3, 3, 1);
    }
    else
    {
      std::stringstream msg;
      msg << "C_abs_.size() <= 1" << std::endl
          << "Manipulator doesn't have enough links." << std::endl;
      throw ahl_utils::Exception("Manipulator::computeJacobian", msg.str());
    }

    Eigen::Matrix3d Pne_cross;
    Pne_cross <<           0.0,  Pne.coeff(2), -Pne.coeff(1),
                 -Pne.coeff(2),           0.0,  Pne.coeff(0),
                  Pne.coeff(1), -Pne.coeff(0),           0.0;
    J_Pne.block(0, 3, 3, 3) = Pne_cross;
    J = J_Pne * J;
  }
}
Beispiel #12
0
 inline bool CheckInBounds(const Eigen::Vector3d& location) const
 {
     return distance_field_.GetImmutable(location.x(), location.y(), location.z()).second;
 }
Beispiel #13
0
float
pcl::visualization::viewScreenArea (
    const Eigen::Vector3d &eye, 
    const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, 
    const Eigen::Matrix4d &view_projection_matrix, int width, int height)
{
  Eigen::Vector4d bounding_box[8];
  bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
  bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
  bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
  bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
  bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
  bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
  bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
  bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);

  // Compute 6-bit code to classify eye with respect to the 6 defining planes
  int pos = ((eye.x () < bounding_box[0].x ()) )  // 1 = left
      + ((eye.x () > bounding_box[6].x ()) << 1)  // 2 = right
      + ((eye.y () < bounding_box[0].y ()) << 2)  // 4 = bottom
      + ((eye.y () > bounding_box[6].y ()) << 3)  // 8 = top
      + ((eye.z () < bounding_box[0].z ()) << 4)  // 16 = front
      + ((eye.z () > bounding_box[6].z ()) << 5); // 32 = back

  // Look up number of vertices
  int num = hull_vertex_table[pos][6];
  if (num == 0)
  {
    return (float (width * height));
  }
    //return 0.0;


//  cout << "eye: " << eye.x() << " " << eye.y() << " " << eye.z() << endl;
//  cout << "min: " << bounding_box[0].x() << " " << bounding_box[0].y() << " " << bounding_box[0].z() << endl;
//
//  cout << "pos: " << pos << " ";
//  switch(pos){
//    case 0:  cout << "inside" << endl; break;
//    case 1:  cout << "left" << endl; break;
//    case 2:  cout << "right" << endl; break;
//    case 3:
//    case 4:  cout << "bottom" << endl; break;
//    case 5:  cout << "bottom, left" << endl; break;
//    case 6:  cout << "bottom, right" << endl; break;
//    case 7:
//    case 8:  cout << "top" << endl; break;
//    case 9:  cout << "top, left" << endl; break;
//    case 10:  cout << "top, right" << endl; break;
//    case 11:
//    case 12:
//    case 13:
//    case 14:
//    case 15:
//    case 16:  cout << "front" << endl; break;
//    case 17:  cout << "front, left" << endl; break;
//    case 18:  cout << "front, right" << endl; break;
//    case 19:
//    case 20:  cout << "front, bottom" << endl; break;
//    case 21:  cout << "front, bottom, left" << endl; break;
//    case 22:
//    case 23:
//    case 24:  cout << "front, top" << endl; break;
//    case 25:  cout << "front, top, left" << endl; break;
//    case 26:  cout << "front, top, right" << endl; break;
//    case 27:
//    case 28:
//    case 29:
//    case 30:
//    case 31:
//    case 32:  cout << "back" << endl; break;
//    case 33:  cout << "back, left" << endl; break;
//    case 34:  cout << "back, right" << endl; break;
//    case 35:
//    case 36:  cout << "back, bottom" << endl; break;
//    case 37:  cout << "back, bottom, left" << endl; break;
//    case 38:  cout << "back, bottom, right" << endl; break;
//    case 39:
//    case 40:  cout << "back, top" << endl; break;
//    case 41:  cout << "back, top, left" << endl; break;
//    case 42:  cout << "back, top, right" << endl; break;
//  }

  //return -1 if inside
  Eigen::Vector2d dst[8];
  for (int i = 0; i < num; i++)
  {
    Eigen::Vector4d world_pt = bounding_box[hull_vertex_table[pos][i]];
    Eigen::Vector2i screen_pt = pcl::visualization::worldToView(world_pt, view_projection_matrix, width, height);
//    cout << "point[" << i << "]: " << screen_pt.x() << " " << screen_pt.y() << endl;
    dst[i] = Eigen::Vector2d(screen_pt.x (), screen_pt.y ());
  }

  double sum = 0.0;
  for (int i = 0; i < num; ++i)
  {
    sum += (dst[i].x () - dst[(i+1) % num].x ()) * (dst[i].y () + dst[(i+1) % num].y ());
  }

  return (fabsf (float (sum * 0.5f)));
}
Beispiel #14
0
int
pcl::visualization::cullFrustum (double frustum[24], const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb)
{
  int result = PCL_INSIDE_FRUSTUM;

  for(int i =0; i < 6; i++){
    double a = frustum[(i*4)];
    double b = frustum[(i*4)+1];
    double c = frustum[(i*4)+2];
    double d = frustum[(i*4)+3];

    //cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl;

    //  Basic VFC algorithm
    Eigen::Vector3d center ((max_bb.x () - min_bb.x ()) / 2 + min_bb.x (),
                            (max_bb.y () - min_bb.y ()) / 2 + min_bb.y (),
                            (max_bb.z () - min_bb.z ()) / 2 + min_bb.z ());

    Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
                            fabs (static_cast<double> (max_bb.y () - center.y ())),
                            fabs (static_cast<double> (max_bb.z () - center.z ())));

    double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
    double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));

    if (m + n < 0){
      result = PCL_OUTSIDE_FRUSTUM;
      break;
    }

    if (m - n < 0)
    {
      result = PCL_INTERSECT_FRUSTUM;
    }
  }

  return result;
}
Beispiel #15
0
//==============================================================================
std::string toString(const Eigen::Vector3d& _v)
{
  return boost::lexical_cast<std::string>(_v.transpose());
}
Beispiel #16
0
void LinearSystem::_BuildSystem(
        LinearSystem*       pLS,
        const unsigned int& StartU,
        const unsigned int& EndU,
        const unsigned int& StartV,
        const unsigned int& EndV
        )
{
    // Jacobian
    Eigen::Matrix<double, 1, 6> BigJ;
    Eigen::Matrix<double, 1, 6> J;

    BigJ.setZero();
    J.setZero();

	// Errors
	double		 e;
	double		 SSD = 0;
    double       Error    = 0;
    unsigned int ErrorPts = 0;

    for( int ii = StartV; ii < EndV; ii++ ) {
        for( int jj = StartU; jj < EndU; jj++ ) {
            // variables
            Eigen::Vector2d Ur;        // pixel position
            Eigen::Vector3d Pr, Pv;    // 3d point
            Eigen::Vector4d Ph;        // homogenized point

            // check if pixel is contained in our model (i.e. has depth)
            if( pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] == 0 ) {
                continue;
            }

            // --------------------- first term 1x2
            // evaluate 'a' = L[ Trv * Linv( Uv ) ]
            // back project to virtual camera's reference frame
            // this already brings points to robotics reference frame
            Pv = pLS->_BackProject( jj, ii, pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] );

            // convert to homogeneous coordinate
            Ph << Pv, 1;

            // transform point to reference camera's frame
            // Pr = Trv * Pv
            Ph = pLS->m_dTrv * Ph;
            Pr = Ph.head( 3 );

            // project onto reference camera
            Eigen::Vector3d Lr;

            Lr = pLS->_Project( Pr );
            Ur = Lr.head( 2 );
            Ur = Ur / Lr( 2 );

            // check if point falls in camera's field of view
            if( (Ur( 0 ) <= 1) || (Ur( 0 ) >= pLS->m_nImgWidth - 2) || (Ur( 1 ) <= 1)
                    || (Ur( 1 ) >= pLS->m_nImgHeight - 2) ) {
                continue;
            }

            // finite differences
            float                       TopPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) - 1, pLS->m_vRefImg );
            float                       BotPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) + 1, pLS->m_vRefImg );
            float                       LeftPix  = pLS->_Interpolate( Ur( 0 ) - 1, Ur( 1 ), pLS->m_vRefImg );
            float                       RightPix = pLS->_Interpolate( Ur( 0 ) + 1, Ur( 1 ), pLS->m_vRefImg );
            Eigen::Matrix<double, 1, 2> Term1;

            Term1( 0 ) = (RightPix - LeftPix) / 2.0;
            Term1( 1 ) = (BotPix - TopPix) / 2.0;

            // --------------------- second term 2x3
            // evaluate 'b' = Trv * Linv( Uv )
            // this was already calculated for Term1
            // fill matrix
            // 1/c      0       -a/c^2
            // 0       1/c     -b/c^2
            Eigen::Matrix<double, 2, 3> Term2;
            double                      PowC = Lr( 2 ) * Lr( 2 );

            Term2( 0, 0 ) = 1.0 / Lr( 2 );
            Term2( 0, 1 ) = 0;
            Term2( 0, 2 ) = -(Lr( 0 )) / PowC;
            Term2( 1, 0 ) = 0;
            Term2( 1, 1 ) = 1.0 / Lr( 2 );
            Term2( 1, 2 ) = -(Lr( 1 )) / PowC;
            Term2         = Term2 * pLS->m_Kr;

            // --------------------- third term 3x1
            // we need Pv in homogenous coordinates
            Ph << Pv, 1;

            Eigen::Vector4d Term3i;

            // last row of Term3 is truncated since it is always 0
            Eigen::Vector3d Term3;

            // fill Jacobian with T generators
            Term3i    = pLS->m_dTrv * pLS->m_Gen[0] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 0 ) = Term1 * Term2 * Term3;
			pLS->m_vJImgX[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 0);
            Term3i    = pLS->m_dTrv * pLS->m_Gen[1] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 1 ) = Term1 * Term2 * Term3;
			pLS->m_vJImgY[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 1);
            Term3i    = pLS->m_dTrv * pLS->m_Gen[2] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 2 ) = Term1 * Term2 * Term3;
			pLS->m_vJImgZ[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 2);
            Term3i    = pLS->m_dTrv * pLS->m_Gen[3] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 3 ) = Term1 * Term2 * Term3;
			pLS->m_vJImgP[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 3);
            Term3i    = pLS->m_dTrv * pLS->m_Gen[4] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 4 ) = Term1 * Term2 * Term3;
			pLS->m_vJImgQ[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 4);
            Term3i    = pLS->m_dTrv * pLS->m_Gen[5] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 5 ) = Term1 * Term2 * Term3;
			pLS->m_vJImgR[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 5);

            // accumulate Jacobian
            BigJ += J;

			// accumulate SSD error
			e = pLS->_Interpolate( Ur( 0 ), Ur( 1 ), pLS->m_vRefImg )
					- pLS->m_vVirtImg[ii * pLS->m_nImgWidth + jj];

			SSD += e * e;

            // calculate normalized error
            Error += fabs( pLS->_Interpolate( Ur( 0 ), Ur( 1 ), pLS->m_vRefImg )
                           - pLS->m_vVirtImg[ii * pLS->m_nImgWidth + jj] );

            ErrorPts++;
        }
    }

    // update global LHS and RHS
    // ---------- start contention zone
    pLS->m_Mutex.lock();

    pLS->m_J         += BigJ;
	pLS->m_dSSD		 += SSD;
    pLS->m_dError    += Error;
    pLS->m_nErrorPts += ErrorPts;

    pLS->m_Mutex.unlock();

    // ---------- end contention zone
}
Beispiel #17
0
void
Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve,
                                                  PolygonMesh &mesh, unsigned resolution)
{
  // copy knots
  if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
  {
    printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
    return;
  }

  mesh.polygons.clear ();

  double x0 = nurbs.Knot (0, 0);
  double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
  double w = x1 - x0;
  double y0 = nurbs.Knot (1, 0);
  double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[1] - 1);
  double h = y1 - y0;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  std::vector<pcl::Vertices> polygons;
  createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
  createIndices (polygons, 0, resolution, resolution);

  std::vector<uint32_t> out_idx;
  pcl::on_nurbs::vector_vec2d out_pc;

  for (unsigned i = 0; i < polygons.size (); i++)
  {
    unsigned in (0);
    pcl::Vertices &poly = polygons[i];

    std::vector<uint32_t> out_idx_tmp;
    pcl::on_nurbs::vector_vec2d out_pc_tmp;

    for (std::size_t j = 0; j < poly.vertices.size (); j++)
    {
      double err;
      Eigen::Vector2d pc, tc;
      uint32_t &vi = poly.vertices[j];
      pcl::PointXYZ &v = cloud->at (vi);
      Eigen::Vector2d vp (v.x, v.y);
      double param = pcl::on_nurbs::FittingCurve2d::findClosestElementMidPoint (curve, vp);
      pcl::on_nurbs::FittingCurve2d::inverseMapping (curve, vp, param, err, pc, tc);
      Eigen::Vector3d a (vp (0) - pc (0), vp (1) - pc (1), 0.0);
      Eigen::Vector3d b (tc (0), tc (1), 0.0);
      Eigen::Vector3d z = a.cross (b);
      if (z (2) >= 0.0)
        in++;
      else
      {
        out_idx_tmp.push_back (vi);
        out_pc_tmp.push_back (pc);
      }
    }

    if (in > 0)
    {
      mesh.polygons.push_back (poly);
      if (in < poly.vertices.size ())
      {
        for (std::size_t j = 0; j < out_idx_tmp.size (); j++)
        {
          out_idx.push_back (out_idx_tmp[j]);
          out_pc.push_back (out_pc_tmp[j]);
        }
      }
    }
  }

  for (std::size_t i = 0; i < out_idx.size (); i++)
  {
    pcl::PointXYZ &v = cloud->at (out_idx[i]);
    Eigen::Vector2d &pc = out_pc[i];
    v.x = pc (0);
    v.y = pc (1);
  }

  for (std::size_t i = 0; i < cloud->size (); i++)
  {
    pcl::PointXYZ &v = cloud->at (i);

    double point[9];
    nurbs.Evaluate (v.x, v.y, 1, 3, point);

    v.x = point[0];
    v.y = point[1];
    v.z = point[2];
  }

  toROSMsg (*cloud, mesh.cloud);
}
Beispiel #18
0
 inline double SlaterSet::pointSlater(SlaterSet *set, const Eigen::Vector3d &delta,
                     const double &dr, unsigned int slater, unsigned int indexMO,
                     double expZeta)
 {
   if (isSmall(set->m_normalized.coeffRef(slater, indexMO))) return 0.0;
   double tmp = set->m_normalized.coeffRef(slater, indexMO) * expZeta;
   // Radial part with effective PQNs
   for (int i = 0; i < set->m_PQNs[slater]; ++i)
     tmp *= dr;
   switch (set->m_slaterTypes[slater]) {
     case S:
       break;
     case PX:
       tmp *= delta.x();
       break;
     case PY:
       tmp *= delta.y();
       break;
     case PZ:
       tmp *= delta.z();
       break;
     case X2: // (x^2 - y^2)r^n
       tmp *= delta.x() * delta.x() - delta.y() * delta.y();
       break;
     case XZ: // xzr^n
       tmp *= delta.x() * delta.z();
       break;
     case Z2: // (2z^2 - x^2 - y^2)r^n
       tmp *= 2.0 * delta.z() * delta.z() - delta.x() * delta.x()
            - delta.y() * delta.y();
       break;
     case YZ: // yzr^n
       tmp *= delta.y() * delta.z();
       break;
     case XY: // xyr^n
       tmp *= delta.x() * delta.y();
       break;
     default:
       return 0.0;
   }
   return tmp;
 }
float PlaneFit::Fit()
{
    _bIsFitted = true;
    if (CountPoints() < 3)
        return FLOAT_MAX;

    double sxx,sxy,sxz,syy,syz,szz,mx,my,mz;
    sxx=sxy=sxz=syy=syz=szz=mx=my=mz=0.0f;

    for (std::list<Base::Vector3f>::iterator it = _vPoints.begin(); it!=_vPoints.end(); ++it) {
        sxx += it->x * it->x; sxy += it->x * it->y;
        sxz += it->x * it->z; syy += it->y * it->y;
        syz += it->y * it->z; szz += it->z * it->z;
        mx  += it->x;   my += it->y;   mz += it->z;
    }

    unsigned int nSize = _vPoints.size();
    sxx = sxx - mx*mx/((double)nSize);
    sxy = sxy - mx*my/((double)nSize);
    sxz = sxz - mx*mz/((double)nSize);
    syy = syy - my*my/((double)nSize);
    syz = syz - my*mz/((double)nSize);
    szz = szz - mz*mz/((double)nSize);

#if defined(FC_USE_BOOST)
    ublas::matrix<double> A(3,3);
    A(0,0) = sxx;
    A(1,1) = syy;
    A(2,2) = szz;
    A(0,1) = sxy; A(1,0) = sxy;
    A(0,2) = sxz; A(2,0) = sxz;
    A(1,2) = syz; A(2,1) = syz;
    namespace lapack= boost::numeric::bindings::lapack;
    ublas::vector<double> eigenval(3);
    int r = lapack::syev('V','U',A,eigenval,lapack::optimal_workspace());
    if (r) {
    }
    float sigma = 0;
#elif defined(FC_USE_EIGEN)
    Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
    covMat(0,0) = sxx;
    covMat(1,1) = syy;
    covMat(2,2) = szz;
    covMat(0,1) = sxy; covMat(1,0) = sxy;
    covMat(0,2) = sxz; covMat(2,0) = sxz;
    covMat(1,2) = syz; covMat(2,1) = syz;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(covMat);

    Eigen::Vector3d u = eig.eigenvectors().col(1);
    Eigen::Vector3d v = eig.eigenvectors().col(2);
    Eigen::Vector3d w = eig.eigenvectors().col(0);

    _vDirU.Set(u.x(), u.y(), u.z());
    _vDirV.Set(v.x(), v.y(), v.z());
    _vDirW.Set(w.x(), w.y(), w.z());
    _vBase.Set(mx/(float)nSize, my/(float)nSize, mz/(float)nSize);

    float sigma = w.dot(covMat * w);
#else
    // Covariance matrix
    Wm4::Matrix3<double> akMat(sxx,sxy,sxz,sxy,syy,syz,sxz,syz,szz);
    Wm4::Matrix3<double> rkRot, rkDiag;
    try {
        akMat.EigenDecomposition(rkRot, rkDiag);
    }
    catch (const std::exception&) {
        return FLOAT_MAX;
    }

    // We know the Eigenvalues are ordered
    // rkDiag(0,0) <= rkDiag(1,1) <= rkDiag(2,2)
    //
    // points describe a line or even are identical
    if (rkDiag(1,1) <= 0)
        return FLOAT_MAX;

    Wm4::Vector3<double> U = rkRot.GetColumn(1);
    Wm4::Vector3<double> V = rkRot.GetColumn(2);
    Wm4::Vector3<double> W = rkRot.GetColumn(0);

    // It may happen that the result have nan values
    for (int i=0; i<3; i++) {
        if (boost::math::isnan(U[i]) || 
            boost::math::isnan(V[i]) ||
            boost::math::isnan(W[i]))
            return FLOAT_MAX;
    }

    _vDirU.Set((float)U.X(), (float)U.Y(), (float)U.Z());
    _vDirV.Set((float)V.X(), (float)V.Y(), (float)V.Z());
    _vDirW.Set((float)W.X(), (float)W.Y(), (float)W.Z());
    _vBase.Set((float)(mx/nSize), (float)(my/nSize), (float)(mz/nSize));
    float sigma = (float)W.Dot(akMat * W);
#endif

    // In case sigma is nan
    if (boost::math::isnan(sigma))
        return FLOAT_MAX;

    // This must be caused by some round-off errors. Theoretically it's impossible
    // that 'sigma' becomes negative because the covariance matrix is positive semi-definite.
    if (sigma < 0)
        sigma = 0;

    // make a right-handed system
    if ((_vDirU % _vDirV) * _vDirW < 0.0f) {
        Base::Vector3f tmp = _vDirU;
        _vDirU = _vDirV;
        _vDirV = tmp;
    }

    if (nSize > 3)
        sigma = sqrt(sigma/(nSize-3));
    _fLastResult = sigma;
    return _fLastResult;
}
Beispiel #20
0
 inline double SlaterSet::calcSlater(SlaterSet *set, const Eigen::Vector3d &delta,
                     const double &dr, unsigned int slater)
 {
   double tmp = set->m_factors[slater] * exp(- set->m_zetas[slater] * dr);
   // Radial part with effective PQNs
   for (int i = 0; i < set->m_PQNs[slater]; ++i)
     tmp *= dr;
   switch (set->m_slaterTypes[slater]) {
     case S:
       break;
     case PX:
       tmp *= delta.x();
       break;
     case PY:
       tmp *= delta.y();
       break;
     case PZ:
       tmp *= delta.z();
       break;
     case X2: // (x^2 - y^2)r^n
       tmp *= delta.x() * delta.x() - delta.y() * delta.y();
       break;
     case XZ: // xzr^n
       tmp *= delta.x() * delta.z();
       break;
     case Z2: // (2z^2 - x^2 - y^2)r^n
       tmp *= 2.0 * delta.z() * delta.z() - delta.x() * delta.x()
            - delta.y() * delta.y();
       break;
     case YZ: // yzr^n
       tmp *= delta.y() * delta.z();
       break;
     case XY: // xyr^n
       tmp *= delta.x() * delta.y();
       break;
     default:
       return 0.0;
   }
   return tmp;
 }
void
FittingCurve2dTDM::assembleInterior (double wInt, double sigma2, unsigned &row)
{
  int nInt = m_data->interior.size ();
  bool wFunction (true);
  double ds = 1.0 / (2.0 * sigma2);
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();
  for (int p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < (int)m_data->interior_param.size ())
    {
      param = inverseMapping (m_nurbs, pcp, m_data->interior_param[p], error, pt, t, in_max_steps, in_accuracy);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint(m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, in_max_steps, in_accuracy);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    // evaluate if point lies inside or outside the closed curve
    Eigen::Vector3d a (pcp (0) - pt (0), pcp (1) - pt (1), 0.0);
    Eigen::Vector3d b (t (0), t (1), 0.0);
    Eigen::Vector3d z = a.cross (b);

    if (p < (int)m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    if (p < (int)m_data->interior_weight_function.size ())
      wFunction = m_data->interior_weight_function[p];

    double w (wInt);
    if (z (2) > 0.0 && wFunction)
      w = wInt * exp (-(error * error) * ds);

    n.normalize ();
    //    m_data->interior_line_start.push_back(pt);
    //    m_data->interior_line_end.push_back(pt + n * 0.01);

    //      w = 0.5 * wInt * exp(-(error * error) * ds);

    // evaluate if this point is the closest point
    //    int idx = NurbsTools::getClosestPoint(pt, m_data->interior);
    //    if(idx == p)
    //      w = 2.0 * wInt;

    if (w > 1e-6) // avoids ill-conditioned matrix
      addPointConstraint (m_data->interior_param[p], m_data->interior[p], n, w, row);
    else
    {
      //      m_solver.K(row, 0, 0.0);
      //      row++;
    }
  }
}
Beispiel #22
0
int main(int argc, char** argv){

  po::options_description desc("./cvt2pcd [options] input_cloud output_pcd");

  desc.add_options()
          ("help", "produce help message")
          ("input,i",po::value<std::string>()->required(), "input point cloud ")
          ("output,o",po::value<std::string>(), "output pcd ")
          ("view_offset,v", po::value< std::string>(), "Offset view offset to translate and convert a float64 to float32 XYZ")
          ("ascii,a", "PCD should be in asci format.  (Default binary compressed)")
          ;

  if (argc <3 ){
           std::cout << "Incorrect number of arguments\n";
           desc.print(std::cout);
           return -1;
   }
  po::positional_options_description p;
  p.add("input",1);
  p.add("output",1);

  po::variables_map vm;
 try{
  po::store(po::command_line_parser(argc, argv).
  options(desc).positional(p).run(), vm);
  po::notify(vm);
 }
 catch( const std::exception& e)
 {
     std::cerr << e.what() << std::endl;
     std::cout << desc << std::endl;
     return 1;
 }

  pcl::CloudReader reader;

  std::string ifile, ofile;
  ifile = vm["input"].as<std::string>();
  if(vm.count("output")) ofile = vm["output"].as<std::string>();
  else {
    ofile = ifile;
    boost::filesystem::path opath = ofile;
    opath.replace_extension(".pcd");
    ofile = opath.string();
  }

#ifdef E57
  std::cout << "Adding E57 Support : \n";
  reader.registerExtension("e57", new pcl::E57Reader( ));
#endif
#ifdef LAS
  reader.registerExtension("las", new pcl::LASReader );
#endif
  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);

  Eigen::Vector4f origin;
  Eigen::Quaternionf rot;

  int fv;
  std::clog << "Loading " <<  ifile << " \n";
  int c = reader.read(ifile,*cloud, origin, rot, fv);
  std::clog << "There were " << c << " points \n";

  if (pcl::hasDoublePointXYZ(*cloud) ){
    if (vm.count("view_offset")){
      Eigen::Vector3d voff;

      if ( 3 == sscanf(vm["view_offset"].as<std::string>().c_str(), "%lf,%lf,%lf", &voff[0], &voff[1], &voff[2]) ){
        std::cout << "Translating by " << voff.transpose() << " \n";
        pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
        pcl::cvtToDoubleAndOffset(*cloud,*cloud2,voff);
        cloud = cloud2;
      }
      else{
        std::cout << "Invalid view offset of : " << vm["view_offset"].as<std::string>() << " \n";
      }
    }
  }


  pcl::PCDWriter writer;
  writer.write(ofile, cloud,origin, rot, !vm.count("ascii"));
  std::cout << ofile << "\n";
}
Beispiel #23
0
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
                                                                                    const typename pcl::search::KdTree<PointT>::Ptr kdtree,
                                                                                    std::vector<Eigen::Matrix3d>& cloud_covariances)
{
  if (k_correspondences_ > int (cloud->size ()))
  {
    PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
    return;
  }

  Eigen::Vector3d mean;
  std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
  std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);

  // We should never get there but who knows
  if(cloud_covariances.size () < cloud->size ())
    cloud_covariances.resize (cloud->size ());

  typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
  std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
  for(;
      points_iterator != cloud->end ();
      ++points_iterator, ++matrices_iterator)
  {
    const PointT &query_point = *points_iterator;
    Eigen::Matrix3d &cov = *matrices_iterator;
    // Zero out the cov and mean
    cov.setZero ();
    mean.setZero ();

    // Search for the K nearest neighbours
    kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
    
    // Find the covariance matrix
    for(int j = 0; j < k_correspondences_; j++) {
      const PointT &pt = (*cloud)[nn_indecies[j]];
      
      mean[0] += pt.x;
      mean[1] += pt.y;
      mean[2] += pt.z;
      
      cov(0,0) += pt.x*pt.x;
      
      cov(1,0) += pt.y*pt.x;
      cov(1,1) += pt.y*pt.y;
      
      cov(2,0) += pt.z*pt.x;
      cov(2,1) += pt.z*pt.y;
      cov(2,2) += pt.z*pt.z;    
    }
  
    mean /= static_cast<double> (k_correspondences_);
    // Get the actual covariance
    for (int k = 0; k < 3; k++)
      for (int l = 0; l <= k; l++) 
      {
        cov(k,l) /= static_cast<double> (k_correspondences_);
        cov(k,l) -= mean[k]*mean[l];
        cov(l,k) = cov(k,l);
      }
    
    // Compute the SVD (covariance matrix is symmetric so U = V')
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
    cov.setZero ();
    Eigen::Matrix3d U = svd.matrixU ();
    // Reconstitute the covariance matrix with modified singular values using the column     // vectors in V.
    for(int k = 0; k < 3; k++) {
      Eigen::Vector3d col = U.col(k);
      double v = 1.; // biggest 2 singular values replaced by 1
      if(k == 2)   // smallest singular value replaced by gicp_epsilon
        v = gicp_epsilon_;
      cov+= v * col * col.transpose(); 
    }
  }
}
Beispiel #24
0
void insituFTSensorCalibrationThread::run()
{
    yarp::os::LockGuard guard(threadMutex);

    if( status == COLLECTING_DATASET )
    {
        double * p_timestamp = 0;
        bool blocking = false;

        if( this->readAccelerationFromSensor )
        {
            sensors->readSensor(wbi::SENSOR_ACCELEROMETER,0,raw_acc[0].data(),p_timestamp,blocking);
        }
        else
        {
            sensors->readSensors(wbi::SENSOR_ENCODER,joint_positions.data(),p_timestamp,blocking);

            wbi::Frame H_world_sensor;
            wbi::Rotation R_sensor_world;
            model->computeH(joint_positions.data(),wbi::Frame::identity(),sensorFrameIndex,H_world_sensor);
            R_sensor_world = H_world_sensor.R.getInverse();

            Eigen::Vector3d g;
            g.setZero();
            g[2] = -9.78;

            Eigen::Map< Eigen::Vector3d >(raw_acc[0].data()) =
            Eigen::Map< Eigen::Matrix<double,3,3,Eigen::RowMajor> >(R_sensor_world.data)*g;
        }

        sensors->readSensor(wbi::SENSOR_FORCE_TORQUE,0,raw_ft[0].data(),p_timestamp,blocking);

        smooth_acc[0] = acc_filters[0]->filt(raw_acc[0]);
        smooth_ft[0] = ft_filters[0]->filt(raw_ft[0]);


        /*
        yDebug("Accelerometer read: %s \n",smooth_acc[0].toString().c_str());
        yDebug("FT read: %s \n",smooth_ft[0].toString().c_str());
        double mass = 4.4850;
        yarp::sig::Vector deb = smooth_acc[0];
        deb[0] = mass*deb[0];
        deb[1] = mass*deb[1];
        deb[2] = mass*deb[2];
        yDebug("Predicted FT read: %s \n",(deb).toString().c_str());*/

        if( this->dump )
        {
            for(int i=0; i < 6; i++ )
            {
                this->datasets_dump << smooth_ft[0][i] << ",";
            }
            this->datasets_dump << smooth_acc[0][0] << ",";
            this->datasets_dump << smooth_acc[0][1] << ",";
            this->datasets_dump << smooth_acc[0][2] << std::endl;
        }

        estimator_datasets[currentDataset]->addMeasurements(InSituFTCalibration::wrapVec(smooth_ft[0]),InSituFTCalibration::wrapVec(smooth_acc[0]));
    }
    else if( status == WAITING_NEW_DATASET_START )
    {
        static int run_count = 0;
        if( run_count % 50 == 0 )
        {
            printf("InSitu FT sensor calibration: waiting for new dataset start.\n");
            printf("Mount the desired added mass and start new dataset collection via the rpc port.\n");
            fflush(stdout);
        }
        run_count++;
    }
}
void test_compute_convex_segments(
    const std::function<void(
      const visualization_msgs::MarkerArray&)>& display_fn)
{
  const double res = 1.0;
  const int64_t x_size = 100;
  const int64_t y_size = 100;
  const int64_t z_size = 50;
  const Eigen::Isometry3d origin_transform
      = Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond(
          Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()));
  sdf_tools::TaggedObjectCollisionMapGrid tocmap(origin_transform, "world", res, x_size, y_size, z_size, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(0.0, 0u));
  for (int64_t x_idx = 0; x_idx < tocmap.GetNumXCells(); x_idx++)
  {
    for (int64_t y_idx = 0; y_idx < tocmap.GetNumYCells(); y_idx++)
    {
      for (int64_t z_idx = 0; z_idx < tocmap.GetNumZCells(); z_idx++)
      {
        if ((x_idx < 10) || (y_idx < 10) || (x_idx >= tocmap.GetNumXCells() - 10) || (y_idx >= tocmap.GetNumYCells() - 10))
        {
          tocmap.SetValue(x_idx, y_idx, z_idx, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(1.0, 1u));
        }
        else if ((x_idx >= 40) && (y_idx >= 40) && (x_idx < 60) && (y_idx < 60))
        {
          tocmap.SetValue(x_idx, y_idx, z_idx, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(1.0, 2u));
        }
        if (((x_idx >= 45) && (x_idx < 55)) || ((y_idx >= 45) && (y_idx < 55)))
        {
          tocmap.SetValue(x_idx, y_idx, z_idx, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(0.0, 0u));
        }
      }
    }
  }
  visualization_msgs::MarkerArray display_markers;
  visualization_msgs::Marker env_marker = tocmap.ExportForDisplay();
  env_marker.id = 1;
  env_marker.ns = "environment";
  display_markers.markers.push_back(env_marker);
  visualization_msgs::Marker components_marker = tocmap.ExportConnectedComponentsForDisplay(false);
  components_marker.id = 1;
  components_marker.ns = "environment_components";
  display_markers.markers.push_back(components_marker);
  const double connected_threshold = 1.75;
  const uint32_t number_of_convex_segments_manual_border = tocmap.UpdateConvexSegments(connected_threshold, false);
  std::cout << "Identified " << number_of_convex_segments_manual_border
            << " convex segments via SDF->maxima map->connected components (no border added)"
            << std::endl;
  for (uint32_t object_id = 0u; object_id <= 4u; object_id++)
  {
    for (uint32_t convex_segment = 1u; convex_segment <= number_of_convex_segments_manual_border; convex_segment++)
    {
      visualization_msgs::Marker segment_marker = tocmap.ExportConvexSegmentForDisplay(object_id, convex_segment);
      if (segment_marker.points.size() > 0)
      {
        segment_marker.ns += "_no_border";
        display_markers.markers.push_back(segment_marker);
      }
    }
  }
  const uint32_t number_of_convex_segments_virtual_border = tocmap.UpdateConvexSegments(connected_threshold, true);
  std::cout << "Identified " << number_of_convex_segments_virtual_border
            << " convex segments via SDF->maxima map->connected components (virtual border added)"
            << std::endl;
  for (uint32_t object_id = 0u; object_id <= 4u; object_id++)
  {
    for (uint32_t convex_segment = 1u; convex_segment <= number_of_convex_segments_virtual_border; convex_segment++)
    {
      visualization_msgs::Marker segment_marker = tocmap.ExportConvexSegmentForDisplay(object_id, convex_segment);
      if (segment_marker.points.size() > 0)
      {
        segment_marker.ns += "_virtual_border";
        display_markers.markers.push_back(segment_marker);
      }
    }
  }
  const auto sdf_result
      = tocmap.ExtractSignedDistanceField(std::numeric_limits<float>::infinity(), std::vector<uint32_t>(), true, false);
  std::cout << "(no border) SDF extrema: " << PrettyPrint::PrettyPrint(sdf_result.second) << std::endl;
  const sdf_tools::SignedDistanceField& sdf = sdf_result.first;
  visualization_msgs::Marker sdf_marker = sdf.ExportForDisplay(1.0f);
  sdf_marker.id = 1;
  sdf_marker.ns = "environment_sdf_no_border";
  display_markers.markers.push_back(sdf_marker);
  const auto virtual_border_sdf_result
      = tocmap.ExtractSignedDistanceField(std::numeric_limits<float>::infinity(), std::vector<uint32_t>(), true, true);
  std::cout << "(virtual border) SDF extrema: " << PrettyPrint::PrettyPrint(virtual_border_sdf_result.second) << std::endl;
  const sdf_tools::SignedDistanceField& virtual_border_sdf = virtual_border_sdf_result.first;
  visualization_msgs::Marker virtual_border_sdf_marker = virtual_border_sdf.ExportForDisplay(1.0f);
  virtual_border_sdf_marker.id = 1;
  virtual_border_sdf_marker.ns = "environment_sdf_virtual_border";
  display_markers.markers.push_back(virtual_border_sdf_marker);
  // Make extrema markers
  const VoxelGrid::VoxelGrid<Eigen::Vector3d> maxima_map = virtual_border_sdf.ComputeLocalExtremaMap();
  for (int64_t x_idx = 0; x_idx < maxima_map.GetNumXCells(); x_idx++)
  {
    for (int64_t y_idx = 0; y_idx < maxima_map.GetNumYCells(); y_idx++)
    {
      for (int64_t z_idx = 0; z_idx < maxima_map.GetNumZCells(); z_idx++)
      {
        const Eigen::Vector4d location
            = maxima_map.GridIndexToLocation(x_idx, y_idx, z_idx);
        const Eigen::Vector3d extrema = maxima_map.GetImmutable(x_idx, y_idx, z_idx).first;
        if (!std::isinf(extrema.x())
            && !std::isinf(extrema.y())
            && !std::isinf(extrema.z()))
        {
          const double distance = (extrema - location.block<3, 1>(0, 0)).norm();
          if (distance < sdf.GetResolution())
          {
            visualization_msgs::Marker maxima_rep;
            // Populate the header
            maxima_rep.header.frame_id = "world";
            // Populate the options
            maxima_rep.ns = "extrema";
            maxima_rep.id = (int32_t)sdf.HashDataIndex(x_idx, y_idx, z_idx);
            maxima_rep.action = visualization_msgs::Marker::ADD;
            maxima_rep.lifetime = ros::Duration(0.0);
            maxima_rep.frame_locked = false;
            maxima_rep.pose.position = EigenHelpersConversions::EigenVector4dToGeometryPoint(location);
            maxima_rep.pose.orientation = EigenHelpersConversions::EigenQuaterniondToGeometryQuaternion(Eigen::Quaterniond::Identity());
            maxima_rep.type = visualization_msgs::Marker::SPHERE;
            maxima_rep.scale.x = sdf.GetResolution();
            maxima_rep.scale.y = sdf.GetResolution();
            maxima_rep.scale.z = sdf.GetResolution();
            maxima_rep.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(1.0, 0.5, 0.0, 1.0);
            display_markers.markers.push_back(maxima_rep);
          }
        }
        else
        {
          std::cout << "Encountered inf extrema @ (" << x_idx << "," << y_idx << "," << z_idx << ")" << std::endl;
        }
      }
    }
  }
  std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetGradient((int64_t)0, (int64_t)0, (int64_t)0, true)) << std::endl;
  std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetGradient((int64_t)1, (int64_t)1, (int64_t)1, true)) << std::endl;
  std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetGradient((int64_t)2, (int64_t)2, (int64_t)2, true)) << std::endl;
  std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetSmoothGradient((int64_t)0, (int64_t)0, (int64_t)0, res)) << std::endl;
  std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetSmoothGradient((int64_t)1, (int64_t)1, (int64_t)1, res)) << std::endl;
  std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetSmoothGradient((int64_t)2, (int64_t)2, (int64_t)2, res)) << std::endl;
  std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetAutoDiffGradient((int64_t)0, (int64_t)0, (int64_t)0)) << std::endl;
  std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetAutoDiffGradient((int64_t)1, (int64_t)1, (int64_t)1)) << std::endl;
  std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetAutoDiffGradient((int64_t)2, (int64_t)2, (int64_t)2)) << std::endl;
  std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(maxima_map.GetImmutable((int64_t)0, (int64_t)0, (int64_t)0).first) << std::endl;
  std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(maxima_map.GetImmutable((int64_t)1, (int64_t)1, (int64_t)1).first) << std::endl;
  std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(maxima_map.GetImmutable((int64_t)2, (int64_t)2, (int64_t)2).first) << std::endl;
  display_fn(display_markers);
}
void BayesNet::createFullJoint(cspace b_Xprior[2]) {

    std::random_device rd;
    std::normal_distribution<double> dist(0, 1);
    cspace tmpConfig;
    for (int i = 0; i < numParticles; i ++) {
        // Root
        for (int j = 0; j < cdim; j++) {
            fullJointPrev[i][j] = b_Xprior[0][j] + b_Xprior[1][j] * (dist(rd));
            tmpConfig[j] = fullJointPrev[i][j];
        }

        // Front Plane
        cspace relativeConfig, baseConfig, transformedConfig, edgeConfig;
        cspace frontPlaneConfig, sidePlaneConfig, otherSidePlaneConfig;
        relativeConfig[0] = 1.22;
        relativeConfig[1] = -0.025;
        relativeConfig[2] = 0;
        relativeConfig[3] = 0;
        relativeConfig[4] = 0;
        relativeConfig[5] = Pi;
        baseConfig = tmpConfig;
        transFrameConfig(baseConfig, relativeConfig, frontPlaneConfig);
        //TEMP:
        if (frontPlaneConfig[5] < 0)  frontPlaneConfig[5] += 2 * Pi;
        copyParticles(frontPlaneConfig, fullJointPrev[i], cdim);

        // Bottom Edge
        cspace prior1[2] = {{0,0,0,1.22,0,0},{0,0,0,0.0005,0.0005,0.0005}};
        for (int j = 0; j < cdim; j++) {
            relativeConfig[j] = prior1[0][j] + prior1[1][j] * (dist(rd));
        }
        baseConfig = tmpConfig;
        transPointConfig(baseConfig, relativeConfig, edgeConfig);
        copyParticles(edgeConfig, fullJointPrev[i], 2 * cdim);

        // Side Edge
        cspace prior2[2] = {{0,-0.025,0,0,-0.025,0.23},{0,0,0,0.0005,0.0005,0.0005}};
        for (int j = 0; j < cdim; j++) {
            relativeConfig[j] = prior2[0][j] + prior2[1][j] * (dist(rd));
        }
        baseConfig = tmpConfig;
        transPointConfig(baseConfig, relativeConfig, transformedConfig);
        copyParticles(transformedConfig, fullJointPrev[i], 3 * cdim);

        // Top edge
        double edgeTol = 0.001;
        double edgeOffSet = 0.23;
        Eigen::Vector3d pa, pb;
        pa << edgeConfig[0], edgeConfig[1], edgeConfig[2];
        pb << edgeConfig[3], edgeConfig[4], edgeConfig[5];
        Eigen::Vector3d pa_prime, pb_prime;
        inverseTransform(pa, frontPlaneConfig, pa_prime);
        inverseTransform(pb, frontPlaneConfig, pb_prime);
        double td = dist(rd) * edgeTol;
        // pa_prime(1) = 0;
        // pb_prime(1) = 0;
        Eigen::Vector3d normVec;
        normVec << (pb_prime(2) - pa_prime(2)), 0, (pa_prime(0) - pb_prime(0));
        normVec.normalize();
        normVec *= (edgeOffSet + td);
        pa_prime(0) += normVec(0);
        pb_prime(0) += normVec(0);
        pa_prime(2) += normVec(2);
        pb_prime(2) += normVec(2);
        Transform(pa_prime, frontPlaneConfig, pa);
        Transform(pb_prime, frontPlaneConfig, pb);
        fullJointPrev[i][24] = pa(0);
        fullJointPrev[i][25] = pa(1);
        fullJointPrev[i][26] = pa(2);
        fullJointPrev[i][27] = pb(0);
        fullJointPrev[i][28] = pb(1);
        fullJointPrev[i][29] = pb(2);

        // Side Plane
        relativeConfig[0] = 0;
        relativeConfig[1] = 0;
        relativeConfig[2] = 0;
        relativeConfig[3] = 0;
        relativeConfig[4] = 0;
        relativeConfig[5] = -Pi / 2.0;
        baseConfig = tmpConfig;
        transFrameConfig(baseConfig, relativeConfig, sidePlaneConfig);
        copyParticles(sidePlaneConfig, fullJointPrev[i], 5 * cdim);

        // Other Side Plane
        relativeConfig[0] = 1.24 + dist(rd) * 0.003;
        // relativeConfig[0] = 1.2192;
        relativeConfig[1] = 0;
        relativeConfig[2] = 0;
        relativeConfig[3] = 0;
        relativeConfig[4] = 0;
        relativeConfig[5] = Pi / 2.0;
        baseConfig = tmpConfig;
        transFrameConfig(baseConfig, relativeConfig, otherSidePlaneConfig);
        copyParticles(otherSidePlaneConfig, fullJointPrev[i], 6 * cdim);
        // Hole


    }
    fullJoint = fullJointPrev;
    Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>((double *)fullJoint.data(), fulldim, numParticles);
    Eigen::MatrixXd mat_centered = mat.colwise() - mat.rowwise().mean();
    cov_mat = (mat_centered * mat_centered.adjoint()) / double(max2(mat.cols() - 1, 1));
}
 void Plane::project(const Eigen::Vector3d& p, Eigen::Vector3d& output)
 {
   double alpha = - p.dot(normal_);
   output = p + alpha * normal_;
 }
Beispiel #28
0
void update_shortest_distance(Eigen::Vector3d new_dist, Eigen::Vector3d& old_dist){
  if (old_dist.norm() > new_dist.norm())
    old_dist = new_dist;
}
 void Line::foot(const Eigen::Vector3d& point, Eigen::Vector3d& output)
 {
   const double alpha = point.dot(direction_) - origin_.dot(direction_);
   output = alpha * direction_ + origin_;
 }
void PositionCommand::getUpdatedPose(const visualeyez_tracker::TrackerPose::ConstPtr& trackerPose)
{
    //ROS_INFO(" Recieved Tracker Location: [%s] [%f] [%f] [%f]",trackerPose->tracker_id.c_str(),trackerPose->pose.x ,trackerPose->pose.y ,trackerPose->pose.z );
    geometry_msgs::Twist vel_cmd_msg;
    ROS_INFO_STREAM(" Recieved Tracker Location: " << trackerPose->tracker_id);

    if(!goal_)
    {
        vel_cmd.publish(vel_cmd_msg);
  return;
    }
    if (base_marker_id_.compare(std::string(trackerPose->tracker_id)) == 0)
    //if(trackerPose->tracker_id==base_marker_id_)
    {
     ROS_INFO_STREAM(" Base Tracker");
        base_marker_position=Eigen::Vector3d(trackerPose->pose.x/1000.0f, trackerPose->pose.y/1000.0f, trackerPose->pose.z/1000.0f);
        got_base_marker_=true;
    }
    else// if(trackerPose->tracker_id==head_marker_id_)
    {
     ROS_INFO_STREAM(" Head Tracker");
        head_marker_position=Eigen::Vector3d(trackerPose->pose.x/1000.0f, trackerPose->pose.y/1000.0f, trackerPose->pose.z/1000.0f);
        got_head_marker_=true;
    }

    // If both markers are available, do the control
    if(got_base_marker_ && got_head_marker_)
    {
if(count==0)
{
ROS_INFO("entrou");
     gettimeofday( &previous_time, NULL );
++count;
           return;
}

        // Define a local reference frame
        Eigen::Vector3d uav_x=(head_marker_position-base_marker_position).normalized(); // Heading

        Eigen::Vector3d uav_y=(uav_x.cross(Eigen::Vector3d::UnitZ())).normalized();
        // LETS ASSUME THE UAV Z POINTS ALWAYS UP


        Eigen::Matrix<double, 3, 3> current_rotation_matrix;
        current_rotation_matrix << uav_x, uav_y, Eigen::Vector3d::UnitZ();
        Eigen::Matrix<double, 3, 1> current_euler = current_rotation_matrix.eulerAngles(2, 1, 0);

        double yaw = current_euler(0,0);
        double pitch = current_euler(1,0);
        double roll = current_euler(2,0);

        current_pose_ << base_marker_position.x(),
                base_marker_position.y(),
                base_marker_position.z(),
                roll,
                pitch,
                yaw;

std::cout << "current_pose: " << current_pose_ << std::endl;
std::cout << "goal_pose: " << goal_pose_ << std::endl;

        Eigen::Matrix<double,6,1> error=goal_pose_-current_pose_;
        // Check if goal was reached
        /*if(equalFloat(current_pose_(0,0), goal_pose_(0,0),epsilon) ||
equalFloat(current_pose_(1,0), goal_pose_(1,0),epsilon) ||
equalFloat(current_pose_(2,0), goal_pose_(2,0),epsilon) ||
equalFloat(current_pose_(3,0), goal_pose_(3,0),epsilon) ||
equalFloat(current_pose_(4,0), goal_pose_(4,0),epsilon) ||
equalFloat(current_pose_(5,0), goal_pose_(5,0),epsilon)
)*/
        if(error.norm()<epsilon) // CHANGE THIS
        {
            ROS_INFO("Reached goal!");
            goal_=false;
            count=0;
        }
        else //control
        {
struct timeval current_time;
   gettimeofday( &previous_time, NULL );

             ROS_INFO_STREAM("position error norm: "<< error.transpose().norm());
   //timespec current_time;
     //clock_gettime(CLOCK_MONOTONIC_RAW, &current_time);
//double period = (double) current_time.tv_sec + (double) 1e-6 * tv.tv_usec;

     //double period = (current_time.tv_sec - previous_time.tv_sec) + ((current_time.tv_usec - previous_time.tv_usec) / 1000000.0);
double period=0.020;
previous_time=current_time;
//std::cout << "time diff: " << period << std::endl;

             //Eigen::Matrix<double,6,6> vel_cmd_current=Kp*(goal_pose_-current_pose_).transpose() + Kd*(current_pose_-previous_pose_).transpose()*freq_;
            Eigen::Matrix<double,6,1> vel_cm_previous_vec;
vel_cm_previous_vec << vel_cmd_previous(0,0), vel_cmd_previous(1,1),vel_cmd_previous(2,2),vel_cmd_previous(3,3),vel_cmd_previous(4,4),vel_cmd_previous(5,5);
Eigen::Matrix<double,6,1> vdiff= ((goal_pose_ - previous_pose_) - (goal_pose_ - vel_cm_previous_vec)); // Differential Part
             Eigen::Matrix<double,6,6> vel_cmd_current=Kp*(goal_pose_-current_pose_).transpose() + Kd*vdiff.transpose();

std::cout << "vdiff: "<<vdiff.transpose() << std::endl;
std::cout << "error: " << (goal_pose_-current_pose_).transpose() << std::endl;
std::cout << "vel_cm_current: "<<vel_cmd_current.transpose() << std::endl;
             vel_cmd_msg.linear.x=vel_cmd_current(0,0);
             vel_cmd_msg.linear.y=vel_cmd_current(1,1);
             vel_cmd_msg.linear.z=vel_cmd_current(2,2);
             //vel_cmd_msg.angular.x=vel_cmd_current(3,3);
             //vel_cmd_msg.angular.y=vel_cmd_current(4,4);
             //vel_cmd_msg.angular.z=vel_cmd_current(5,5);
             vel_cmd.publish(vel_cmd_msg);

vel_cmd_previous=vel_cmd_current;
        }

        previous_pose_=current_pose_;

    }else
     {
     gettimeofday( &previous_time, NULL );
     }

}