void fuzzyAffines()
{
std::vector<Eigen::Matrix4f> trans;
trans.reserve(count/10);
for( size_t i=0; i<count/10; i++ )
{
Eigen::Vector3f x = Eigen::Vector3f::Random();
Eigen::Vector3f y = Eigen::Vector3f::Random();
x.normalize();
y.normalize();
Eigen::Vector3f z = x.cross(y);
z.normalize();
y = z.cross(x);
y.normalize();
Eigen::Affine3f t = Eigen::Affine3f::Identity();
Eigen::Matrix3f r = Eigen::Matrix3f::Identity();
r.col(0) = x;
r.col(1) = y;
r.col(2) = z;
t.rotate(r);
t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );
trans.push_back( t.matrix() );
}
s_plot.setColor( Eigen::Vector4f(1,0,0,1) );
s_plot.setLineWidth( 3.0 );
s_plot( trans, nox::plot<float>::Pos | nox::plot<float>::CS );
}
void SensorFusion::reset(const Eigen::Vector3f& accel, const Eigen::Vector3f& magnetom)
{
G_Dt = 0;
Eigen::Vector3f temp1;
Eigen::Vector3f temp2;
Eigen::Vector3f xAxis;
xAxis << 1.0f, 0.0f, 0.0f;
timestamp = highResClock.now();
// GET PITCH
// Using y-z-plane-component/x-component of gravity vector
pitch = -atan2f(accel(0), sqrtf(accel(1) * accel(1) + accel(2) * accel(2)));
// GET ROLL
// Compensate pitch of gravity vector
temp1 = accel.cross(xAxis);
temp2 = xAxis.cross(temp1);
// Normally using x-z-plane-component/y-component of compensated gravity vector
// roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
// Since we compensated for pitch, x-z-plane-component equals z-component:
roll = atan2f(temp2(1), temp2(2));
// GET YAW
compassHeading(magnetom);
yaw = magHeading;
// Init rotation matrix
init_rotation_matrix(dcmMatrix, yaw, pitch, roll);
}
// http://www.scratchapixel.com/lessons/3d-basic-rendering/ray-tracing-rendering-a-triangle/ray-triangle-intersection-geometric-solution
float Triangle::checkHit(Eigen::Vector3f eye, Eigen::Vector3f dir) {
double u, v, t;
// first check for circumsphere hit
Eigen::Vector3f dist = eye - center;
double A = dot(dir, dir);
double B = dot((2*dir), dist);
double C = dot(dist, dist) - radius*radius;
Eigen::Vector3f quad = QuadraticFormula(A, B, C);
float result;
if (quad(0) == 0) {
//SHOULD BE AN ERROR
result = 0;
}
if (quad(0) == 1) {
result = quad(1);
}
if (fabs(quad(1)) <= fabs(quad(2))) {
result = quad(1);
} else {
result = quad(2);
}
// failure to even hit the circumsphere
if (result < 0) {
return 0;
}
Eigen::Vector3f ab = b - a;
Eigen::Vector3f ac = c - a;
Eigen::Vector3f pvec = dir.cross(ac);
double det = dot(ab, pvec);
#ifdef CULLING
// if the determinant is negative the triangle is backfacing
// if the determinant is close to 0, the ray misses the triangle
if (det < kEpsilon) return 0;
#else
// ray and triangle are parallel if det is close to 0
if (fabs(det) < kEpsilon) return 0;
#endif
double invDet = 1 / det;
Eigen::Vector3f tvec = eye - a;
u = dot(tvec, pvec) * invDet;
if (u < 0 || u > 1) return 0;
Eigen::Vector3f qvec = tvec.cross(ab);
v = dot(dir, qvec) * invDet;
if (v < 0 || u + v > 1) return 0;
t = dot(ac, qvec) * invDet;
return t;
}
void stateCallback(const nav_msgs::Odometry::ConstPtr& state)
{
vel[0] = state->twist.twist.linear.x;
vel[1] = state->twist.twist.linear.y;
vel[2] = state->twist.twist.linear.z;
pos[2] = state->pose.pose.position.z;
//q.x() = state->pose.pose.orientation.x;
//q.y() = state->pose.pose.orientation.y;
//q.z() = state->pose.pose.orientation.z;
//q.w() = state->pose.pose.orientation.w;
float q_x = state->pose.pose.orientation.x;
float q_y = state->pose.pose.orientation.y;
float q_z = state->pose.pose.orientation.z;
float q_w = state->pose.pose.orientation.w;
float yaw = atan2(2*(q_w*q_z+q_x*q_y),1-2*(q_y*q_y+q_z*q_z));
//Eigen::Matrix3d R(q);
force(0) = offset_x+k_vxy*(vel_des(0)-vel(0))+m*acc_des(0);
if(pd_control==0)
force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+m*acc_des(1);
else if(pd_control==1)
{
pos[1]=state->pose.pose.position.y;
force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+k_xy*(0-pos[1])+m*acc_des(1);
}
force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2));
b3 = force.normalized();
b2 = b3.cross(b1w);
b1 = b2.cross(b3);
R_des<<b1[0],b2[0],b3[0],
b1[1],b2[1],b3[1],
b1[2],b2[2],b3[2];
Eigen::Quaternionf q_des(R_des);
quadrotor_msgs::SO3Command command;
command.force.x = force[0];
command.force.y = force[1];
command.force.z = force[2];
command.orientation.x = q_des.x();
command.orientation.y = q_des.y();
command.orientation.z = q_des.z();
command.orientation.w = q_des.w();
command.kR[0] = k_R;
command.kR[1] = k_R;
command.kR[2] = k_R;
command.kOm[0] = k_omg;
command.kOm[1] = k_omg;
command.kOm[2] = k_omg;
command.aux.current_yaw = yaw;
command.aux.enable_motors = true;
command.aux.use_external_yaw = true;
control_pub.publish(command);
}
Eigen::Vector3f lineNormalized(Eigen::Vector3f p0, Eigen::Vector3f p1)
{
Eigen::Vector3f l = p0.cross(p1);
l.x() = l.x() / l.z();
l.y() = l.y() / l.z();
l.z() = 1.0f;
//return l;
return p0.cross(p1).normalized();
}
void MsgToPoint3D(const TPPLPoint &pt, const cob_3d_mapping_msgs::Shape::ConstPtr& new_message, Eigen::Vector3f &pos, Eigen::Vector3f &normal) {
if(new_message->params.size()==4) {
Eigen::Vector3f u,v,origin;
Eigen::Affine3f transformation;
normal(0)=new_message->params[0];
normal(1)=new_message->params[1];
normal(2)=new_message->params[2];
origin(0)=new_message->centroid.x;
origin(1)=new_message->centroid.y;
origin(2)=new_message->centroid.z;
//std::cout << "normal: " << normal << std::endl;
//std::cout << "centroid: " << origin << std::endl;
v = normal.unitOrthogonal ();
u = normal.cross (v);
pcl::getTransformationFromTwoUnitVectorsAndOrigin(v, normal, origin, transformation);
transformation=transformation.inverse();
Eigen::Vector3f p3;
p3(0)=pt.x;
p3(1)=pt.y;
p3(2)=0;
pos = transformation*p3;
}
else if(new_message->params.size()==5) {
Eigen::Vector2f v,v2,n2;
v(0)=pt.x;
v(1)=pt.y;
v2=v;
v2(0)*=v2(0);
v2(1)*=v2(1);
n2(0)=new_message->params[3];
n2(1)=new_message->params[4];
//dummy normal
normal(0)=new_message->params[0];
normal(1)=new_message->params[1];
normal(2)=new_message->params[2];
Eigen::Vector3f x,y, origin;
x(0)=1.f;
y(1)=1.f;
x(1)=x(2)=y(0)=y(2)=0.f;
Eigen::Matrix<float,3,2> proj2plane_;
proj2plane_.col(0)=normal.cross(y);
proj2plane_.col(1)=normal.cross(x);
origin(0)=new_message->centroid.x;
origin(1)=new_message->centroid.y;
origin(2)=new_message->centroid.z;
pos = origin+proj2plane_*v + normal*(v2.dot(n2));
normal += normal*(v).dot(n2);
}
}
/**
* @brief Computes the trackball's rotation, using stored initial and final position vectors.
*/
void computeRotationAngle (void)
{
//Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.
if(initialPosition.norm() > 0) {
initialPosition.normalize();
}
if(finalPosition.norm() > 0) {
finalPosition.normalize();
}
//cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;
Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);
if(rotationAxis.norm() != 0) {
rotationAxis.normalize();
}
float dot = initialPosition.dot(finalPosition);
float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.
Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));
quaternion = q * quaternion;
quaternion.normalize();
}
boost::optional<Intersection> Triangle::intersect(Ray ray) {
Eigen::Vector3f s1 = ray.dir.cross(d2);
const float div = s1.dot(d1);
if(div <= 0) { // parallel or back
return boost::optional<Intersection>();
}
const float div_inv = 1 / div;
Eigen::Vector3f s = ray.org - p0;
const float a = s.dot(s1) * div_inv;
if(a < 0 || a > 1) {
return boost::optional<Intersection>();
}
Eigen::Vector3f s2 = s.cross(d1);
const float b = ray.dir.dot(s2) * div_inv;
if(b < 0 || a + b > 1) {
return boost::optional<Intersection>();
}
const float t = d2.dot(s2) * div_inv;
if(t < 0) {
return boost::optional<Intersection>();
}
return boost::optional<Intersection>(Intersection(
t,
ray.at(t),
normal,
(1 - a - b) * uv0 + a * uv1 + b * uv2,
(1 - a - b) * ir0 + a * ir1 + b * ir2,
attribute));
}
bool SnapIt::checkPointInsidePlane(EigenVector3fVector &plane_points,
Eigen::Vector3f normal,
Eigen::Vector3f point)
{
if (isnan(point[0]) || isnan(point[1]) || isnan(point[2])) {
return false;
}
for (size_t i = 0; i < plane_points.size(); i++) {
Eigen::Vector3f B;
Eigen::Vector3f O = plane_points[i];
if (i == (plane_points.size() - 1)) {
B = plane_points[0];
}
else {
B = plane_points[i + 1];
}
Eigen::Vector3f OB = B - O;
Eigen::Vector3f OP = point - O;
if ((OB.cross(OP)).dot(normal) < 0) {
return false;
}
}
return true;
}
bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
Eigen::Affine3f& transf)
{
// uz: versor pointing toward the destination
Eigen::Vector3f uz = target - rayo;
if (std::abs(uz.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
return false;
}
uz.normalize();
//std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
// ux: versor pointing toward the ground
Eigen::Vector3f ux = down - down.dot(uz) * uz;
if (std::abs(ux.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
return false;
}
ux.normalize();
//std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
Eigen::Vector3f uy = uz.cross(ux);
//std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
Eigen::Matrix3f rot;
rot << ux.x(), uy.x(), uz.x(),
ux.y(), uy.y(), uz.y(),
ux.z(), uy.z(), uz.z();
transf.setIdentity();
transf.translate(rayo);
transf.rotate(rot);
//std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
return true;
}
bool
VirtualTrackball::mouseMoved ( const MouseEvent* event )
{
if ( !event->isLeftButtonPressed() ) return false;
const Eigen::Vector3f p0 = this->_oldp;
const Eigen::Vector3f p1 = this->project_on_sphere ( event->x(), event->y() );
this->_oldp = p1;
if ( ( p0 - p1 ).norm() < this->_eps ) return false; // do nothing
//何か間違ってそうなので訂正してみる
float radian = std::acos( p0.dot ( p1 ) )*0.5;
const float cost = std::cos(radian);
const float sint = std::sin(radian);
//const float cost = p0.dot ( p1 );
//const float sint = std::sqrt ( 1 - cost * cost );
const Eigen::Vector3f axis = p0.cross ( p1 ).normalized();
const Eigen::Quaternionf q ( -cost, sint * axis.x(), sint * axis.y(), sint * axis.z() );
if( ( q.x()!=q.x() )|| ( q.y()!=q.y() )|| ( q.z()!=q.z() )|| ( q.w()!=q.w() ) ) return false;
/*
Eigen::Vector3f bmin , bmax;
Mesh mesh;
mesh = this->_model.getMesh();
mesh.getBoundingBox(bmin,bmax);
Eigen::Vector3f mc = (bmin + bmax)*0.5;
Eigen::Vector3f c = Eigen::Matrix3f(q) * ( this->_model.getCamera().getCenter() - mc ) + mc ;
this->_model.setCameraPosition(c.x(),c.y(),c.z());*/
//this->_model.addRotation ( q );
return true;
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::selectBaseTriangle (std::vector <int> &base_indices)
{
int nr_points = static_cast <int> (target_indices_->size ());
float best_t = 0.f;
// choose random first point
base_indices[0] = (*target_indices_)[rand () % nr_points];
int *index1 = &base_indices[0];
// random search for 2 other points (as far away as overlap allows)
for (int i = 0; i < ransac_iterations_; i++)
{
int *index2 = &(*target_indices_)[rand () % nr_points];
int *index3 = &(*target_indices_)[rand () % nr_points];
Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap ();
Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap ();
float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal
// check for most suitable point triple
if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
{
best_t = t;
base_indices[1] = *index2;
base_indices[2] = *index3;
}
}
// return if a triplet could be selected
return (best_t == 0.f ? -1 : 0);
}
Transform<float, 3, Affine, AutoAlign> Plane::get2DArbitraryRefSystem() const
{
Eigen::Vector3f n = normal_.getUnitNormal();
/// seek for a good unit axis to project on plane
/// and then use it as X direction of the 2d local system
size_t min_id;
n.array().abs().minCoeff(&min_id);
DLOG(INFO) << "min id is " << min_id;
Vector3f proj_axis = Vector3f::Zero();
proj_axis(min_id) = 1; // unity on that axis
// project the selected axis on the plane
Vector3f second_ax = projectVectorOnPlane(proj_axis);
second_ax.normalize();
Vector3f first_ax = n.cross(second_ax);
first_ax.normalize();
Transform<float, 3, Affine, AutoAlign> T;
// T.matrix().fill(0);
T.matrix().col(0).head(3) = first_ax;
T.matrix().col(1).head(3) = second_ax;
T.matrix().col(2).head(3) = n;
// T.matrix()(3, 3) = 1;
DLOG(INFO) << "Transform computed \n " << T.inverse().matrix() << "normal was " << n;
DLOG(INFO) << "In fact T*n " <<T.inverse() *n;
return T.inverse();
}
Eigen::Matrix4f ForwardKinematicsLiego::getEpsilon(Eigen::Vector3f omega,
Eigen::Vector3f q) {
Eigen::Matrix4f eps = getDash(omega);
omega = -omega.cross(q);
eps(0, 3) = omega[0];
eps(1, 3) = omega[1];
eps(2, 3) = omega[2];
return eps;
}
void compute_plane(Eigen::Vector4f& plane, const pcl::PointCloud<pcl::PointXYZ>& points, int* inds)
{
Eigen::Vector3f first = points[inds[1]].getVector3fMap() - points[inds[0]].getVector3fMap();
Eigen::Vector3f second = points[inds[2]].getVector3fMap() - points[inds[0]].getVector3fMap();
Eigen::Vector3f normal = first.cross(second);
normal.normalize();
plane.segment<3>(0) = normal;
plane(3) = -normal.dot(points[inds[0]].getVector3fMap());
}
// Rotate the Vector 'normal_to_rotate' into 'base_normal'
// Returns a rotation matrix which can be used for the transformation
// The matrix also includes an empty translation vector
Eigen::Matrix< float, 4, 4 > rotateAroundCrossProductOfNormals(
Eigen::Vector3f base_normal,
Eigen::Vector3f normal_to_rotate)
{
// M
// Eigen::Vector3f plane_normal(dest.x, dest.y, dest.z);
// Compute the necessary rotation to align a face of the object with the camera's
// imaginary image plane
// N
// Eigen::Vector3f camera_normal;
// camera_normal(0)=0;
// camera_normal(1)=0;
// camera_normal(2)=1;
// Eigen::Vector3f camera_normal_normalized = camera_normal.normalized();
float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );
Eigen::Vector3f axis;
Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
// axis = plane_normal.cross(camera_normal) / (plane_normal.cross(camera_normal)).normalize();
firstAxis.normalize();
axis=firstAxis;
float c = costheta;
std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
float s = sqrt(1-c*c);
float CO = 1-c;
float x = axis(0);
float y = axis(1);
float z = axis(2);
Eigen::Matrix< float, 4, 4 > rotationBox;
rotationBox(0,0) = x*x*CO+c;
rotationBox(1,0) = y*x*CO+z*s;
rotationBox(2,0) = z*x*CO-y*s;
rotationBox(0,1) = x*y*CO-z*s;
rotationBox(1,1) = y*y*CO+c;
rotationBox(2,1) = z*y*CO+x*s;
rotationBox(0,2) = x*z*CO+y*s;
rotationBox(1,2) = y*z*CO-x*s;
rotationBox(2,2) = z*z*CO+c;
// Translation vector
rotationBox(0,3) = 0;
rotationBox(1,3) = 0;
rotationBox(2,3) = 0;
// The rest of the 4x4 matrix
rotationBox(3,0) = 0;
rotationBox(3,1) = 0;
rotationBox(3,2) = 0;
rotationBox(3,3) = 1;
return rotationBox;
}
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinNucleusCentroid (const Eigen::Vector3f& nucleus,
const Eigen::Vector3f& centroid,
const Eigen::Vector3f& nc,
const PointInT& point) const
{
Eigen::Vector3f pc = centroid - point.getVector3fMap ();
Eigen::Vector3f pn = nucleus - point.getVector3fMap ();
Eigen::Vector3f pc_cross_nc = pc.cross (nc);
return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0));
}
Eigen::Matrix4f lookAt(const Eigen::Vector3f &eye,
const Eigen::Vector3f ¢er,
const Eigen::Vector3f &up) {
Eigen::Vector3f f = (center - eye).normalized();
Eigen::Vector3f s = f.cross(up).normalized();
Eigen::Vector3f u = s.cross(f);
Eigen::Matrix4f Result = Eigen::Matrix4f::Identity();
Result(0, 0) = s(0);
Result(0, 1) = s(1);
Result(0, 2) = s(2);
Result(1, 0) = u(0);
Result(1, 1) = u(1);
Result(1, 2) = u(2);
Result(2, 0) = -f(0);
Result(2, 1) = -f(1);
Result(2, 2) = -f(2);
Result(0, 3) = -s.transpose() * eye;
Result(1, 3) = -u.transpose() * eye;
Result(2, 3) = f.transpose() * eye;
return Result;
}
template<typename PointInT, typename PointNT, typename PointOutT> float
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::getAngleBetweenUnitVectors (
Eigen::Vector3f const &v1,
Eigen::Vector3f const &v2,
Eigen::Vector3f const &axis)
{
Eigen::Vector3f angle_orientation;
angle_orientation = v1.cross (v2);
float angle_radians = acosf (std::max (-1.0f, std::min (1.0f, v1.dot (v2))));
angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast<float> (M_PI) - angle_radians) : angle_radians;
return (angle_radians);
}
Eigen::Matrix< float, 4, 4 > IAMethod::rotateAroundCrossProductOfNormals(
Eigen::Vector3f base_normal,
Eigen::Vector3f normal_to_rotate,
bool store_transformation)
{
normal_to_rotate *= -1; // The model is standing upside down, rotate the normal by 180 DEG
float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );
Eigen::Vector3f axis;
Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
firstAxis.normalize();
axis=firstAxis;
float c = costheta;
std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
float s = sqrt(1-c*c);
float CO = 1-c;
float x = axis(0);
float y = axis(1);
float z = axis(2);
Eigen::Matrix< float, 4, 4 > rotationBox;
rotationBox(0,0) = x*x*CO+c;
rotationBox(1,0) = y*x*CO+z*s;
rotationBox(2,0) = z*x*CO-y*s;
rotationBox(0,1) = x*y*CO-z*s;
rotationBox(1,1) = y*y*CO+c;
rotationBox(2,1) = z*y*CO+x*s;
rotationBox(0,2) = x*z*CO+y*s;
rotationBox(1,2) = y*z*CO-x*s;
rotationBox(2,2) = z*z*CO+c;
// Translation vector
rotationBox(0,3) = 0;
rotationBox(1,3) = 0;
rotationBox(2,3) = 0;
// The rest of the 4x4 matrix
rotationBox(3,0) = 0;
rotationBox(3,1) = 0;
rotationBox(3,2) = 0;
rotationBox(3,3) = 1;
if(store_transformation)
rotations_.push_back(rotationBox);
return rotationBox;
}
template<typename PointT> bool
pcl::CropHull<PointT>::rayTriangleIntersect (const PointT& point,
const Eigen::Vector3f& ray,
const Vertices& verts,
const PointCloud& cloud)
{
// Algorithm here is adapted from:
// http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
//
// Original copyright notice:
// Copyright 2001, softSurfer (www.softsurfer.com)
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
//
assert (verts.vertices.size () == 3);
const Eigen::Vector3f p = point.getVector3fMap ();
const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
const Eigen::Vector3f u = b - a;
const Eigen::Vector3f v = c - a;
const Eigen::Vector3f n = u.cross (v);
const float n_dot_ray = n.dot (ray);
if (std::fabs (n_dot_ray) < 1e-9)
return (false);
const float r = n.dot (a - p) / n_dot_ray;
if (r < 0)
return (false);
const Eigen::Vector3f w = p + r * ray - a;
const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
const float s = s_numerator / denominator;
if (s < 0 || s > 1)
return (false);
const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
const float t = t_numerator / denominator;
if (t < 0 || s+t > 1)
return (false);
return (true);
}
bool GenericIsometricBendingConstraint::initConstraint(SimulationModel &model, const unsigned int particle1, const unsigned int particle2,
const unsigned int particle3, const unsigned int particle4)
{
m_bodies[0] = particle3;
m_bodies[1] = particle4;
m_bodies[2] = particle1;
m_bodies[3] = particle2;
ParticleData &pd = model.getParticles();
const Eigen::Vector3f &x0 = pd.getPosition0(m_bodies[0]);
const Eigen::Vector3f &x1 = pd.getPosition0(m_bodies[1]);
const Eigen::Vector3f &x2 = pd.getPosition0(m_bodies[2]);
const Eigen::Vector3f &x3 = pd.getPosition0(m_bodies[3]);
// Compute matrix Q for quadratic bending
const Eigen::Vector3f *x[4] = { &x0, &x1, &x2, &x3 };
const Eigen::Vector3f e0 = *x[1] - *x[0];
const Eigen::Vector3f e1 = *x[2] - *x[0];
const Eigen::Vector3f e2 = *x[3] - *x[0];
const Eigen::Vector3f e3 = *x[2] - *x[1];
const Eigen::Vector3f e4 = *x[3] - *x[1];
const float c01 = MathFunctions::cotTheta(e0, e1);
const float c02 = MathFunctions::cotTheta(e0, e2);
const float c03 = MathFunctions::cotTheta(-e0, e3);
const float c04 = MathFunctions::cotTheta(-e0, e4);
const float A0 = 0.5f * (e0.cross(e1)).norm();
const float A1 = 0.5f * (e0.cross(e2)).norm();
const float coef = -3.f / (2.f*(A0 + A1));
const float K[4] = { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 };
const float K2[4] = { coef*K[0], coef*K[1], coef*K[2], coef*K[3] };
for (unsigned char j = 0; j < 4; j++)
{
for (unsigned char k = 0; k < j; k++)
{
m_Q(j, k) = m_Q(k, j) = K[j] * K2[k];
}
m_Q(j, j) = K[j] * K2[j];
}
return true;
}
std::pair<Eigen::Vector3f, Eigen::Vector3f>
Utils::generateAxes(const Eigen::Hyperplane<float, 3> &plane_,
const Eigen::Vector3f &point_)
{
Eigen::Vector3f normal = plane_.normal();
// Take an arbitrary direction from the plane's origin (OUTSIDE the plane)
Eigen::Vector3f u = point_ + Eigen::Vector3f(1E15, 1E15, 1E15);
// Project that arbitrary point into the plane to get the first axis inside the plane
Eigen::Vector3f v1 = plane_.projection(u).normalized();
// Generate the second unitary vector
Eigen::Vector3f v2 = normal.cross(v1).normalized();
// Return the axes
return std::pair<Eigen::Vector3f, Eigen::Vector3f>(v1, v2);
}
bool SimpleRayCaster::intersectRayTriangle(const Eigen::Vector3f ray,
const Eigen::Vector3f a, const Eigen::Vector3f b, const Eigen::Vector3f c,
Eigen::Vector3f& isec)
{
/* As discribed by:
* http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle%28%29 */
const Eigen::Vector3f p(0,0,0);
const Eigen::Vector3f u = b - a;
const Eigen::Vector3f v = c - a;
const Eigen::Vector3f n = u.cross(v);
const float n_dot_ray = n.dot(ray);
if (std::fabs(n_dot_ray) < 1e-9) {
return false;
}
const float r = n.dot(a-p) / n_dot_ray;
if (r < 0) {
return false;
}
// the ray intersection point
isec = ray * r;
const Eigen::Vector3f w = p + r * ray - a;
const float denominator = u.dot(v) * u.dot(v) - u.dot(u) * v.dot(v);
const float s_numerator = u.dot(v) * w.dot(v) - v.dot(v) * w.dot(u);
const float s = s_numerator / denominator;
if (s < 0 || s > 1) {
return false;
}
const float t_numerator = u.dot(v) * w.dot(u) - u.dot(u) * w.dot(v);
const float t = t_numerator / denominator;
if (t < 0 || s+t > 1) {
return false;
}
return true;
}
cv::Point2d FindObjectOnPlane::getUyEnd(
const cv::Point2d& ux_start,
const cv::Point2d& ux_end,
const image_geometry::PinholeCameraModel& model,
const jsk_recognition_utils::Plane::Ptr& plane)
{
cv::Point3d start_ray = model.projectPixelTo3dRay(ux_start);
cv::Point3d end_ray = model.projectPixelTo3dRay(ux_end);
Eigen::Vector3f ux_start_3d = rayPlaneInteersect(start_ray, plane);
Eigen::Vector3f ux_end_3d = rayPlaneInteersect(end_ray, plane);
Eigen::Vector3f ux_3d = ux_end_3d - ux_start_3d;
Eigen::Vector3f normal = plane->getNormal();
Eigen::Vector3f uy_3d = normal.cross(ux_3d).normalized();
Eigen::Vector3f uy_end_3d = ux_start_3d + uy_3d;
cv::Point2d uy_end = model.project3dToPixel(cv::Point3d(uy_end_3d[0],
uy_end_3d[1],
uy_end_3d[2]));
return uy_end;
}
Camera::Camera()
#ifdef _SMALL_SCENE
:
w(100),
h(100),
eye(/*arma::fvec4("0 0.5 2 1")*/Eigen::Vector3f(0,0.5,2)),
look(/*arma::fvec4("0 0.5 0 1")*/Eigen::Vector3f(0,0.5,0)),
#else
:
w(600),
h(600),
eye(/*arma::fvec4("0 5 20 1")*/Eigen::Vector3f(0,5,20)),
look(/*arma::fvec4("0 5 0 1")*/Eigen::Vector3f(0,5,0)),
#endif // _SMALL_SCENE
up(/*arma::fvec4("0 1 0 0")*/Eigen::Vector3f(0, 1, 0)),
fov(45.f)
{
Eigen::Vector3f dir = eye - look;
yDir = /*hp.normalise4v*/(up.normalized());
xDir = dir.cross(yDir).normalized();/*-hp.normalise4v(hp.cross4v(dir, yDir));*/
//xDir.print("xDir");
//yDir.print("yDir");
float xRange = dir.norm() * tan(fov * M_PI / 180. / 2);
float yRange = xRange / w * h;
std::cout << "xr = " << xRange << std::endl;
std::cout << "yr = " << yRange << std::endl;
dx = xRange / w * 2;
dy = yRange / h * 2;
std::cout << "dx = " << dx << std::endl;
std::cout << "dy = " << dy << std::endl;
ori = look - xRange * xDir - yRange * yDir;
//ori.print("ori");
}
void computeTransformationFromYZVectorsAndOrigin(const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis,
const Eigen::Vector3f& origin, Eigen::Affine3f& transformation){
Eigen::Vector3f x = (y_direction.cross(z_axis)).normalized();
Eigen::Vector3f y = y_direction.normalized();
Eigen::Vector3f z = z_axis.normalized();
Eigen::Affine3f sub = Eigen::Affine3f::Identity();
sub(0,3) = -origin[0];
sub(1,3) = -origin[1];
sub(2,3) = -origin[2];
transformation = Eigen::Affine3f::Identity();
transformation(0,0)=x[0]; transformation(0,1)=x[1]; transformation(0,2)=x[2]; // x^t
transformation(1,0)=y[0]; transformation(1,1)=y[1]; transformation(1,2)=y[2]; // y^t
transformation(2,0)=z[0]; transformation(2,1)=z[1]; transformation(2,2)=z[2]; // z^t
transformation = transformation*sub;
}
/**
* @brief Sensor::getInplaneTransform
* @param pt
* @param normal
* @param pose
*/
void Sensor::getInplaneTransform(const Eigen::Vector3f &pt, const Eigen::Vector3f &normal, Eigen::Matrix4f &pose)
{
pose.setIdentity();
Eigen::Vector3f px, py;
Eigen::Vector3f pz = normal;
if (pt.dot(pz) > 0) pz *= -1;
px = (Eigen::Vector3f(1,0,0).cross(pz)).normalized();
py = (pz.cross(px)).normalized();
pose.block<3,1>(0,0) = px;
pose.block<3,1>(0,1) = py;
pose.block<3,1>(0,2) = pz;
pose.block<3,1>(0,3) = pt;
// std::vector<Eigen::Vector3f> pts0(4), pts1(4);
// std::vector<int> indices(4,0);
// indices[1] = 1, indices[2] = 2, indices[3] = 3;
// pts0[0] = Eigen::Vector3f(0,0,0), pts0[1] = Eigen::Vector3f(1,0,0);
// pts0[2] = Eigen::Vector3f(0,1,0), pts0[3] = Eigen::Vector3f(0,0,1);
// pts1[0] = pt;
// pts1[3] = normal.normalized();
// if (pts1[0].dot(pts1[3]) > 0)
// pts1[3] *= -1;
// pts1[1] = (pts0[1].cross(pts1[3])).normalized();
// pts1[2] = (pts1[3].cross(pts1[1])).normalized();
// pts1[1]+=pts1[0];
// pts1[2]+=pts1[0];
// pts1[3]+=pts1[0];
// v4r::RigidTransformationRANSAC rt;
// rt.estimateRigidTransformationSVD(pts0, indices, pts1, indices, pose);
}
Eigen::MatrixXf CoMIK::getJacobianOfCoM(RobotNodePtr node)
{
// Get number of degrees of freedom
size_t nDoF = rns->getAllRobotNodes().size();
// Create matrices for the position and the orientation part of the jacobian.
Eigen::MatrixXf position = Eigen::MatrixXf::Zero(3, nDoF);
const std::vector<RobotNodePtr> parentsN = bodyNodeParents[node];
// Iterate over all degrees of freedom
for (size_t i = 0; i < nDoF; i++)
{
RobotNodePtr dof = rns->getNode(i);// bodyNodes[i];
// Check if the tcp is affected by this DOF
if (find(parentsN.begin(), parentsN.end(), dof) != parentsN.end())
{
// Calculus for rotational joints is different as for prismatic joints.
if (dof->isRotationalJoint())
{
// get axis
boost::shared_ptr<RobotNodeRevolute> revolute
= boost::dynamic_pointer_cast<RobotNodeRevolute>(dof);
THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
// todo: find a better way of handling different joint types
Eigen::Vector3f axis = revolute->getJointRotationAxis(coordSystem);
// For CoM-Jacobians only the positional part is necessary
Eigen::Vector3f toTCP = node->getCoMLocal() + node->getGlobalPose().block(0, 3, 3, 1)
- dof->getGlobalPose().block(0, 3, 3, 1);
position.block(0, i, 3, 1) = axis.cross(toTCP);
}
else if (dof->isTranslationalJoint())
{
// -> prismatic joint
boost::shared_ptr<RobotNodePrismatic> prismatic
= boost::dynamic_pointer_cast<RobotNodePrismatic>(dof);
THROW_VR_EXCEPTION_IF(!prismatic, "Internal error: expecting prismatic joint");
// todo: find a better way of handling different joint types
Eigen::Vector3f axis = prismatic->getJointTranslationDirection(coordSystem);
position.block(0, i, 3, 1) = axis;
}
}
}
if (target.rows() == 2)
{
Eigen::MatrixXf result(2, nDoF);
result.row(0) = position.row(0);
result.row(1) = position.row(1);
return result;
}
else if (target.rows() == 1)
{
VR_INFO << "One dimensional CoMs not supported." << endl;
}
return position;
}
template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
PointInTPtr & grid, pcl::PointIndices & indices)
{
Eigen::Vector3f plane_normal;
plane_normal[0] = -centroid[0];
plane_normal[1] = -centroid[1];
plane_normal[2] = -centroid[2];
Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
plane_normal.normalize ();
Eigen::Vector3f axis = plane_normal.cross (z_vector);
double rotation = -asin (axis.norm ());
axis.normalize ();
Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
grid->points.resize (processed->points.size ());
for (size_t k = 0; k < processed->points.size (); k++)
grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();
pcl::transformPointCloud (*grid, *grid, transformPC);
Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
centroid4f = transformPC * centroid4f;
normal_centroid4f = transformPC * normal_centroid4f;
Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
Eigen::Vector4f farthest_away;
pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
farthest_away[3] = 0;
float max_dist = (farthest_away - centroid4f).norm ();
pcl::demeanPointCloud (*grid, centroid4f, *grid);
Eigen::Matrix4f center_mat;
center_mat.setIdentity (4, 4);
center_mat (0, 3) = -centroid4f[0];
center_mat (1, 3) = -centroid4f[1];
center_mat (2, 3) = -centroid4f[2];
Eigen::Matrix3f scatter;
scatter.setZero ();
float sum_w = 0.f;
//for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++)
for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap ();
float d_k = (pvector).norm ();
float w = (max_dist - d_k);
Eigen::Vector3f diff = (pvector);
Eigen::Matrix3f mat = diff * diff.transpose ();
scatter = scatter + mat * w;
sum_w += w;
}
scatter /= sum_w;
Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
Eigen::Vector3f evx = svd.matrixV ().col (0);
Eigen::Vector3f evy = svd.matrixV ().col (1);
Eigen::Vector3f evz = svd.matrixV ().col (2);
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
Eigen::Vector3f evzminus = evz * -1;
float s_xplus, s_xminus, s_yplus, s_yminus;
s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
//disambiguate rf using all points
for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
float dist_x, dist_y;
dist_x = std::abs (evx.dot (pvector));
dist_y = std::abs (evy.dot (pvector));
if ((pvector).dot (evx) >= 0)
s_xplus += dist_x;
else
s_xminus += dist_x;
if ((pvector).dot (evy) >= 0)
s_yplus += dist_y;
else
s_yminus += dist_y;
}
if (s_xplus < s_xminus)
evx = evxminus;
if (s_yplus < s_yminus)
evy = evyminus;
//select the axis that could be disambiguated more easily
float fx, fy;
float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
float min_y = static_cast<float> (std::min (s_yplus, s_yminus));
fx = (min_x / max_x);
fy = (min_y / max_y);
Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
if (normal3f.dot (evz) < 0)
evz = evzminus;
//if fx/y close to 1, it was hard to disambiguate
//what if both are equally easy or difficult to disambiguate, namely fy == fx or very close
float max_axis = std::max (fx, fy);
float min_axis = std::min (fx, fy);
if ((min_axis / max_axis) > axis_ratio_)
{
PCL_WARN("Both axis are equally easy/difficult to disambiguate\n");
Eigen::Vector3f evy_copy = evy;
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
if (min_axis > min_axis_value_)
{
//combination of all possibilities
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evxminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evyminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
else
{
//1-st case (evx selected)
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
//2-nd case (evy selected)
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
}
else
{
if (fy < fx)
{
evx = evy;
fx = fy;
}
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
return true;
}
