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 ();
}
//==============================================================================
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;
}
}
inline bool CheckInBounds(const Eigen::Vector3d& location) const
{
return distance_field_.GetImmutable(location.x(), location.y(), location.z()).second;
}
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)));
}
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;
}
//==============================================================================
std::string toString(const Eigen::Vector3d& _v)
{
return boost::lexical_cast<std::string>(_v.transpose());
}
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
}
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);
}
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;
}
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++;
}
}
}
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";
}
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();
}
}
}
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_;
}
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, ¤t_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 );
}
}