/**
* @brief Computes the trackball's rotation, using stored initial and final position vectors.
*/
void computeRotationAngle (void)
{
//Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.
if(initialPosition.norm() > 0) {
initialPosition.normalize();
}
if(finalPosition.norm() > 0) {
finalPosition.normalize();
}
//cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;
Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);
if(rotationAxis.norm() != 0) {
rotationAxis.normalize();
}
float dot = initialPosition.dot(finalPosition);
float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.
Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));
quaternion = q * quaternion;
quaternion.normalize();
}
float RobotNodeSet::getMaximumExtension()
{
float result = 0;
Eigen::Matrix4f t;
Eigen::Vector3f v;
if (kinematicRoot && robotNodes.size()>0)
{
t = kinematicRoot->getTransformationTo(robotNodes[0]);
v = MathTools::getTranslation(t);
result += v.norm();
}
for (size_t i=0;i<this->robotNodes.size()-1;i++)
{
t = robotNodes[i]->getTransformationTo(robotNodes[i+1]);
v = MathTools::getTranslation(t);
result += v.norm();
}
if (tcp && robotNodes.size()>0)
{
t = tcp->getTransformationTo(robotNodes[robotNodes.size()-1]);
v = MathTools::getTranslation(t);
result += v.norm();
}
return result;
}
float robotDepth()
{
// Two interesting points
float alpha1 = (x + (sx-height)/2)/(float)height*2*tan(VFOV/2.);
float alpha2 = (x - (sx+height)/2)/(float)height*2*tan(VFOV/2.);
float beta = (y - width/2)/(float)width*2*tan(HFOV/2.);
// Vectors
Eigen::Vector3f v1(1,-beta,-alpha1);
Eigen::Vector3f v2(1,-beta,-alpha2);
// Normalization
v1 = v1/v1.norm();
v2 = v2/v2.norm();
// Center
Eigen::Vector3f c = (v1+v2)/2.;
float c_norm = c.norm();
// Projection
Eigen::MatrixXf proj_mat = c.transpose()*v1;
float proj = proj_mat(0,0);
// Orthogonal part in v1
Eigen::Vector3f orth = v1 - proj/c_norm*c;
// Norm
float orth_norm = orth.norm();
// Approximate depth
float d = H/2.*proj/orth_norm;
return d;
}
bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
Eigen::Affine3f& transf)
{
// uz: versor pointing toward the destination
Eigen::Vector3f uz = target - rayo;
if (std::abs(uz.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
return false;
}
uz.normalize();
//std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
// ux: versor pointing toward the ground
Eigen::Vector3f ux = down - down.dot(uz) * uz;
if (std::abs(ux.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
return false;
}
ux.normalize();
//std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
Eigen::Vector3f uy = uz.cross(ux);
//std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
Eigen::Matrix3f rot;
rot << ux.x(), uy.x(), uz.x(),
ux.y(), uy.y(), uz.y(),
ux.z(), uy.z(), uz.z();
transf.setIdentity();
transf.translate(rayo);
transf.rotate(rot);
//std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
return true;
}
// Rotate the Vector 'normal_to_rotate' into 'base_normal'
// Returns a rotation matrix which can be used for the transformation
// The matrix also includes an empty translation vector
Eigen::Matrix< float, 4, 4 > rotateAroundCrossProductOfNormals(
Eigen::Vector3f base_normal,
Eigen::Vector3f normal_to_rotate)
{
// M
// Eigen::Vector3f plane_normal(dest.x, dest.y, dest.z);
// Compute the necessary rotation to align a face of the object with the camera's
// imaginary image plane
// N
// Eigen::Vector3f camera_normal;
// camera_normal(0)=0;
// camera_normal(1)=0;
// camera_normal(2)=1;
// Eigen::Vector3f camera_normal_normalized = camera_normal.normalized();
float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );
Eigen::Vector3f axis;
Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
// axis = plane_normal.cross(camera_normal) / (plane_normal.cross(camera_normal)).normalize();
firstAxis.normalize();
axis=firstAxis;
float c = costheta;
std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
float s = sqrt(1-c*c);
float CO = 1-c;
float x = axis(0);
float y = axis(1);
float z = axis(2);
Eigen::Matrix< float, 4, 4 > rotationBox;
rotationBox(0,0) = x*x*CO+c;
rotationBox(1,0) = y*x*CO+z*s;
rotationBox(2,0) = z*x*CO-y*s;
rotationBox(0,1) = x*y*CO-z*s;
rotationBox(1,1) = y*y*CO+c;
rotationBox(2,1) = z*y*CO+x*s;
rotationBox(0,2) = x*z*CO+y*s;
rotationBox(1,2) = y*z*CO-x*s;
rotationBox(2,2) = z*z*CO+c;
// Translation vector
rotationBox(0,3) = 0;
rotationBox(1,3) = 0;
rotationBox(2,3) = 0;
// The rest of the 4x4 matrix
rotationBox(3,0) = 0;
rotationBox(3,1) = 0;
rotationBox(3,2) = 0;
rotationBox(3,3) = 1;
return rotationBox;
}
bool Vis::collidingWith(const Object& object) const
{
Eigen::Vector3f const swimDirection(goalPos - this->pos),
objectDirection(object.pos - this->pos);
float const swimDistance = swimDirection.norm(),
objectDistance = objectDirection.norm();
if (objectDistance < (this->collisionRadius * this->scale + object.collisionRadius * object.scale))
{
/*
* We're using this theorem from the dot product, u and v are
* vectors, T is the smallest angle between those vectors.
*
* u · v = |u| |v| cos T
*
* Thus:
* u · v
* cos T = ---------
* |u| |v|
*
* Additionally, given that T is always the smallest angle,
* thus T is always <= π. Also cos x / dx for 0 <= x <= π is
* always <= 0. Thus for S,T in [0,Ï€] S < T, S > T and S = T
* imply cos S < cos T, cos S > T and cos S = T respectively.
*
* Thus while we cannot use cos T as T, we can use it to
* compare the angle T with another angle S. Hence we don't
* need to find T by obtaining arccos cos T, which we would be
* quite computationally expensive.
*/
float const cos_angle = swimDirection.dot(objectDirection)
/ (swimDistance * objectDistance);
/*
* Only act on our collision if *this* is the object crashing
* right into it, i.e. our angle with that object is less than
* 90Ëš. If our angle is greater than 90Ëš then we are in fact
* already traveling away from the colliding object. For angles
* x from 0Ëš to 90Ëš cos(x) ranges from 1 to 0, i.e. remains
* positive.
*/
if (cos_angle >= 0.f)
{
collided = collisionVisualLength;
return true;
}
}
return false;
}
Eigen::Matrix< float, 4, 4 > IAMethod::rotateAroundCrossProductOfNormals(
Eigen::Vector3f base_normal,
Eigen::Vector3f normal_to_rotate,
bool store_transformation)
{
normal_to_rotate *= -1; // The model is standing upside down, rotate the normal by 180 DEG
float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );
Eigen::Vector3f axis;
Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
firstAxis.normalize();
axis=firstAxis;
float c = costheta;
std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
float s = sqrt(1-c*c);
float CO = 1-c;
float x = axis(0);
float y = axis(1);
float z = axis(2);
Eigen::Matrix< float, 4, 4 > rotationBox;
rotationBox(0,0) = x*x*CO+c;
rotationBox(1,0) = y*x*CO+z*s;
rotationBox(2,0) = z*x*CO-y*s;
rotationBox(0,1) = x*y*CO-z*s;
rotationBox(1,1) = y*y*CO+c;
rotationBox(2,1) = z*y*CO+x*s;
rotationBox(0,2) = x*z*CO+y*s;
rotationBox(1,2) = y*z*CO-x*s;
rotationBox(2,2) = z*z*CO+c;
// Translation vector
rotationBox(0,3) = 0;
rotationBox(1,3) = 0;
rotationBox(2,3) = 0;
// The rest of the 4x4 matrix
rotationBox(3,0) = 0;
rotationBox(3,1) = 0;
rotationBox(3,2) = 0;
rotationBox(3,3) = 1;
if(store_transformation)
rotations_.push_back(rotationBox);
return rotationBox;
}
bool Pcl_grabing::checkForHand()
{
update_kinect_points();
int npts = transformed_pclKinect_clr_ptr_->points.size();
Eigen::Vector3f pt;
Eigen::Vector3f dist;
double distance = 1;
vector<int> index;
index.clear();
for (int i = 0; i < npts; i++)
{
pt = transformed_pclKinect_clr_ptr_->points[i].getVector3fMap();
dist = pt - TableCentroid;
dist[2]=0;
distance = dist.norm();
if(distance < TableRadius)
{
if(pt[2]>(TableHeight+HandMinHeight))
{
index.push_back(i);
}
}
}
int n_hand_points = index.size();
if(n_hand_points<20)
{
ROS_INFO("there is no hand.");
return 0;
}
ROS_INFO("got hand.");
return true;
}
size_t closest_point_index_rayOMP(const pcl::PointCloud<PointT>& cloud,
const Eigen::Vector3f& direction_pre,
const Eigen::Vector3f& line_pt,
double& distance_to_ray) {
Eigen::Vector3f direction = direction_pre / direction_pre.norm();
PointT point;
std::vector<double> distances(cloud.points.size(), 10); // Initialize to value 10
// Generate a vector of distances
#pragma omp parallel for
for (size_t index = 0; index < cloud.points.size(); index++) {
point = cloud.points[index];
Eigen::Vector3f cloud_pt(point.x, point.y, point.z);
Eigen::Vector3f difference = (line_pt - cloud_pt);
// https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation
double distance = (difference - (difference.dot(direction) * direction)).norm();
distances[index] = distance;
}
double min_distance = std::numeric_limits<double>::infinity();
size_t min_index = 0;
// Find index of maximum (TODO: Figure out how to OMP this)
for (size_t index = 0; index < cloud.points.size(); index++) {
const double distance = distances[index];
if (distance < min_distance) {
min_distance = distance;
min_index = index;
}
}
distance_to_ray = min_distance;
return (min_index);
}
float cPointLight::Shadow(const cScene &scene, const Eigen::Vector3f &p, Eigen::Vector3f &l) const
{
l = pos - p;
float Dist = l.norm();
float attenuation = DistScale / Dist;
l /= Dist;
cRay ray(l, p); // shadow ray
sMaterial Texture;
// check all occluding objects
attenuation = attenuation * attenuation;// distance attenuation is prop. to squared dist.
for (std::pair<const aWorldObject *, float> occlude = scene.intersect(ray, Dist);
occlude.first && occlude.second < Dist;
occlude = scene.intersect(ray, Dist))
{
ray.orig = ray.point(occlude.second);
Texture = occlude.first->getMaterialAt(ray.orig);
if (Texture.mKTransp < cRayTracer::mThreshold) // object is opaque
return 0;
if ((attenuation *= Texture.mKTransp) < cRayTracer::mThreshold)
return 0;
Dist -= occlude.second;
}
return attenuation;
}
bool DihedralConstraint::initConstraint(SimulationModel &model, const unsigned int particle1, const unsigned int particle2,
const unsigned int particle3, const unsigned int particle4)
{
m_bodies[0] = particle1;
m_bodies[1] = particle2;
m_bodies[2] = particle3;
m_bodies[3] = particle4;
ParticleData &pd = model.getParticles();
const Eigen::Vector3f &p0 = pd.getPosition0(particle1);
const Eigen::Vector3f &p1 = pd.getPosition0(particle2);
const Eigen::Vector3f &p2 = pd.getPosition0(particle3);
const Eigen::Vector3f &p3 = pd.getPosition0(particle4);
Eigen::Vector3f e = p3 - p2;
float elen = e.norm();
if (elen < 1e-6f)
return false;
float invElen = 1.0f / elen;
Eigen::Vector3f n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.squaredNorm();
Eigen::Vector3f n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.squaredNorm();
n1.normalize();
n2.normalize();
float dot = n1.dot(n2);
if (dot < -1.0f) dot = -1.0f;
if (dot > 1.0f) dot = 1.0f;
m_restAngle = acosf(dot);
return true;
}
bool SimoxRobotViewer::showVector( const std::string &vecName, const Eigen::Vector3f &pos, const Eigen::Vector3f &ori, float scaling )
{
removeVector(vecName);
lock();
SoSeparator* sep = new SoSeparator();
sep->addChild(CoinVisualizationFactory::CreateVertexVisualization(pos,5,0,1,0,0));
if (ori.norm()>1e-9 && scaling>0)
{
SoTranslation* t = new SoTranslation();
//cout << "ori:\n" << ori << endl;
t->translation.setValue(pos[0],pos[1],pos[2]);
sep->addChild(t);
SoSeparator* sepA = CoinVisualizationFactory::CreateArrow(ori,50.0f*scaling,2.0f*scaling,VirtualRobot::VisualizationFactory::Color::Blue());
sep->addChild(sepA);
}
SoSeparator* sText = CoinVisualizationFactory::CreateText(vecName);
if (sText)
sep->addChild(sText);
vectors[vecName] = sep;
// add visu
sceneSep->addChild(sep);
unlock();
return true;
}
bool SlamEngine::IsTransformationBigEnough()
{
if (!using_optimizer)
{
return false;
}
if (accumulated_frame_count >= Config::instance()->get<int>("max_keyframe_interval"))
{
return true;
}
Eigen::Vector3f t = TranslationFromMatrix4f(accumulated_transformation);
float tnorm = t.norm();
Eigen::Vector3f e = EulerAngleFromQuaternion(QuaternionFromMatrix4f(accumulated_transformation));
e *= 180.0 / M_PI;
float max_angle = std::max(fabs(e(0)), std::max(fabs(e(1)), fabs(e(2))));
cout << ", " << tnorm << ", " << max_angle;
if (tnorm > Config::instance()->get<float>("min_translation_meter")
|| max_angle > Config::instance()->get<float>("min_rotation_degree"))
{
return true;
}
return false;
}
template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
const Eigen::Vector3f& origin,
const Eigen::Vector3f& end,
AlignedPointTVector &voxel_center_list,
float precision)
{
Eigen::Vector3f direction = end - origin;
float norm = direction.norm ();
direction.normalize ();
const float step_size = static_cast<const float> (resolution_) * precision;
// Ensure we get at least one step for the first voxel.
const int nsteps = std::max (1, static_cast<int> (norm / step_size));
OctreeKey prev_key;
bool bkeyDefined = false;
// Walk along the line segment with small steps.
for (int i = 0; i < nsteps; ++i)
{
Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
PointT octree_p;
octree_p.x = p.x ();
octree_p.y = p.y ();
octree_p.z = p.z ();
OctreeKey key;
this->genOctreeKeyforPoint (octree_p, key);
// Not a new key, still the same voxel.
if ((key == prev_key) && (bkeyDefined) )
continue;
prev_key = key;
bkeyDefined = true;
PointT center;
genLeafNodeCenterFromOctreeKey (key, center);
voxel_center_list.push_back (center);
}
OctreeKey end_key;
PointT end_p;
end_p.x = end.x ();
end_p.y = end.y ();
end_p.z = end.z ();
this->genOctreeKeyforPoint (end_p, end_key);
if (!(end_key == prev_key))
{
PointT center;
genLeafNodeCenterFromOctreeKey (end_key, center);
voxel_center_list.push_back (center);
}
return (static_cast<int> (voxel_center_list.size ()));
}
void display(void)
{
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
for(float i = 0; i<width-1; i+=1){
for(float j = 0; j<width-1; j+=1){
glBegin(GL_POLYGON);
Mass m1 = *masses[i*width+j];
Mass m2 = *masses[i*width+j+1];
Mass m3 = *masses[(i+1)*width+j+1];
Mass m4 = *masses[(i+1)*width+j];
Eigen::Vector3f m11;
Eigen::Vector3f m12;
Eigen::Vector3f m13;
Eigen::Vector3f m14;
Eigen::Vector3f n;
m11 << m1.position.x(),m1.position.y(),m1.position.z();
m12 << m2.position.x(),m2.position.y(),m2.position.z();
m14 << m4.position.x(),m4.position.y(),m4.position.z();
n = (m11-m12).cross(m11-m14);
n = n/n.norm();
glNormal3f(n.x(),n.y(),n.z());
glVertex3f(m1.position.x(),m1.position.y(),m1.position.z());
glNormal3f(n.x(),n.y(),n.z());
glVertex3f(m2.position.x(),m2.position.y(),m2.position.z());
glNormal3f(n.x(),n.y(),n.z());
glVertex3f(m4.position.x(),m4.position.y(),m4.position.z());
glEnd();
glBegin(GL_POLYGON);
m12 << m2.position.x(),m2.position.y(),m2.position.z();
m13 << m3.position.x(),m3.position.y(),m3.position.z();
m14 << m4.position.x(),m4.position.y(),m4.position.z();
n = -(m12-m13).cross(m12-m14);
n = n/n.norm();
glNormal3f(n.x(),n.y(),n.z());
glVertex3f(m2.position.x(),m2.position.y(),m2.position.z());
glNormal3f(n.x(),n.y(),n.z());
glVertex3f(m3.position.x(),m3.position.y(),m3.position.z());
glNormal3f(n.x(),n.y(),n.z());
glVertex3f(m4.position.x(),m4.position.y(),m4.position.z());
glEnd();
}
}
glFlush ();
}
/** Update this line.
* @param linfo new info to consume
* This also updates moving averages for all fields.
*/
void TrackedLineInfo::update(LineInfo &linfo)
{
if (visibility_history <= 0)
visibility_history = 1;
else
visibility_history += 1;
this->raw = linfo;
fawkes::tf::Stamped<fawkes::tf::Point> bp_new(
fawkes::tf::Point(
linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
), fawkes::Time(0,0), input_frame_id);
try {
transformer->transform_point(tracking_frame_id, bp_new, this->base_point_odom);
} catch (fawkes::tf::TransformException &e) {
logger->log_warn(plugin_name.c_str(), "Can't transform to %s. Attempting to track in %s.",
tracking_frame_id.c_str(), input_frame_id.c_str());
this->base_point_odom = bp_new;
}
this->history.push_back(linfo);
Eigen::Vector3f base_point_sum(0,0,0), end_point_1_sum(0,0,0),
end_point_2_sum(0,0,0), line_direction_sum(0,0,0), point_on_line_sum(0,0,0);
float length_sum(0);
for (LineInfo &l : this->history) {
base_point_sum += l.base_point;
end_point_1_sum += l.end_point_1;
end_point_2_sum += l.end_point_2;
line_direction_sum += l.line_direction;
point_on_line_sum += l.point_on_line;
length_sum += l.length;
}
size_t sz = this->history.size();
this->smooth.base_point = base_point_sum / sz;
this->smooth.cloud = linfo.cloud;
this->smooth.end_point_1 = end_point_1_sum / sz;
this->smooth.end_point_2 = end_point_2_sum / sz;
this->smooth.length = length_sum / sz;
this->smooth.line_direction = line_direction_sum / sz;
this->smooth.point_on_line = point_on_line_sum / sz;
Eigen::Vector3f x_axis(1,0,0);
Eigen::Vector3f ld_unit = this->smooth.line_direction / this->smooth.line_direction.norm();
Eigen::Vector3f pol_invert = Eigen::Vector3f(0,0,0) - this->smooth.point_on_line;
Eigen::Vector3f P = this->smooth.point_on_line + pol_invert.dot(ld_unit) * ld_unit;
this->smooth.bearing = std::acos(x_axis.dot(P) / P.norm());
// we also want to encode the direction of the angle
if (P[1] < 0)
this->smooth.bearing = std::abs(this->smooth.bearing) * -1.;
Eigen::Vector3f l_diff = raw.end_point_2 - raw.end_point_1;
Eigen::Vector3f l_ctr = raw.end_point_1 + l_diff / 2.;
this->bearing_center = std::acos(x_axis.dot(l_ctr) / l_ctr.norm());
if (l_ctr[1] < 0)
this->bearing_center = std::abs(this->bearing_center) * -1.;
}
bool Camera::collides(const Entity &e) {
// return false;
float cam_rad = collisionRadius();
if (e.useBoundingBox()) {
// return true;
BoundingBox bb = e.getBoundingBox();
bb.box.min += e.getPosition();
bb.box.max += e.getPosition();
Eigen::Vector3f our_pos = -translations;
if (bb.contains(our_pos)) {
return true;
}
Eigen::Vector3f displacement = (bb.box.center() - our_pos);
float distance = displacement.norm();
Eigen::Vector3f direction = displacement.normalized();
if (bb.contains(direction * cam_rad + our_pos)) {
return true;
}
return false;
}
else {
Eigen::Vector3f their_pos = e.getCenterWorld();
Eigen::Vector3f our_pos = translations;
their_pos(1) = 0;
our_pos(1) = 0;
Eigen::Vector3f delta = -their_pos - our_pos;
return std::abs(delta.norm()) < cam_rad + e.getRadius();
}
/*
std::cout << "object pos" << std::endl;
std::cout << their_pos << std::endl;
std::cout << "cam pos" << std::endl;
std::cout << our_pos << std::endl;
std::cout << " dist = " << std::abs(delta.norm()) << std::endl;
*/
}
inline void
RangeImagePlanar::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const
{
Eigen::Vector3f transformedPoint = to_range_image_system_ * point;
range = transformedPoint.norm ();
image_x = center_x_ + focal_length_x_*transformedPoint[0]/transformedPoint[2];
image_y = center_y_ + focal_length_y_*transformedPoint[1]/transformedPoint[2];
}
inline void
RangeImageSpherical::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const
{
Eigen::Vector3f transformedPoint = to_range_image_system_ * point;
range = transformedPoint.norm ();
float angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]),
angle_y = asinLookUp (transformedPoint[1]/range);
getImagePointFromAngles (angle_x, angle_y, image_x, image_y);
}
bool CUDACameraTrackingMultiResRGBD::checkRigidTransformation(Eigen::Matrix3f& R, Eigen::Vector3f& t, float angleThres, float distThres) {
Eigen::AngleAxisf aa(R);
if (aa.angle() > angleThres || t.norm() > distThres) {
std::cout << "Tracking lost: angle " << (aa.angle()/M_PI)*180.0f << " translation " << t.norm() << std::endl;
return false;
}
return true;
}
void ImuDeadReckon::makeQuaternionFromVector( Eigen::Vector3f& inVec, Eigen::Quaternionf& outQuat )
{
float phi = inVec.norm();
Eigen::Vector3f u = inVec / phi; // u is a unit vector
outQuat.vec() = u * sin( phi / 2.0 );
outQuat.w() = cos( phi / 2.0 );
//outQuat = Eigen::Quaternionf( 1.0, 0., 0., 0. );
}
double angular_difference(geometry_msgs::Quaternion c,geometry_msgs::Quaternion d){
Eigen::Vector4f dv;
dv[0] = d.w; dv[1] = d.x; dv[2] = d.y; dv[3] = d.z;
Eigen::Matrix<float, 3,4> inv;
inv(0,0) = -c.x; inv(0,1) = c.w; inv(0,2) = -c.z; inv(0,3) = c.y;
inv(1,0) = -c.y; inv(1,1) = c.z; inv(1,2) = c.w; inv(1,3) = -c.x;
inv(2,0) = -c.z; inv(2,1) = -c.y;inv(2,2) = c.x; inv(2,3) = c.w;
Eigen::Vector3f m = inv * dv * -2.0;
return m.norm();
}
Eigen::Vector3f PhotoCamera::unproject(const Eigen::Vector2f &pixel)
{
Eigen::Vector3f pixelH;
pixelH << pixel(0), pixel(1), 1;
Eigen::MatrixXf pseudoInverse = getPseudoInverse();
Eigen::Vector4f XH = pseudoInverse*pixelH;
Eigen::Vector3f p = XH.hnormalized();
Eigen::Vector3f ray = p - cameraCenter.hnormalized();
ray /= ray.norm();
return ray;
}
void RobotNodePrismatic::setVisuScaleFactor(Eigen::Vector3f& scaleFactor)
{
if (scaleFactor.norm() == 0.0f)
{
visuScaling = false;
}
else
{
visuScaling = true;
visuScaleFactor = scaleFactor;
}
}
void Render::Arrow::draw(Render::Style style) const
{
Eigen::Vector3f point;
Render::Cylinder cylinder;
Render::Cone cone;
point = terminus_ - origin_;
point = point * (point.norm() - 0.3f) / point.norm();
point = point + origin_;
cylinder.setVertex1(origin_);
cylinder.setVertex2(point);
cylinder.setRadius(radius_);
cylinder.setMaterial(material_);
cylinder.draw(style, 24);
cone.setVertex1(point);
cone.setVertex2(terminus_);
cone.setRadius(radius_ * 2);
cone.setMaterial(material_);
cone.draw(style);
}
void CoaxVisionControl::StateCallback(const coax_msgs::CoaxState::ConstPtr & msg) {
static int initTime = 200;
static int initCounter = 0;
static Eigen::Vector3f init_acc(0,0,0);
static Eigen::Vector3f init_gyr(0,0,0);
battery_voltage = msg->battery;
coax_nav_mode = msg->mode.navigation;
// rpy << msg->roll, msg->pitch, msg->yaw;
// std::cout << "RPY: \n" << rpy << std::endl;
accel << msg->accel[0], msg->accel[1], msg->accel[2];
gyro << msg->gyro[0], msg->gyro[1], msg->gyro[2];
rpyt_rc << msg->rcChannel[4], msg->rcChannel[6], msg->rcChannel[2], msg->rcChannel[0];
rpyt_rc_trim << msg->rcChannel[5], msg->rcChannel[7], msg->rcChannel[3], msg->rcChannel[1];
range_al = msg->zfiltered;
global_z = msg->zfiltered;
if (FIRST_STATE) {
last_state_time = ros::Time::now().toSec();
FIRST_STATE = false;
ROS_INFO("First Time Stamp: %f",last_state_time);
return;
}
if ((battery_voltage < 10.50) && !LOW_POWER_DETECTED) {
ROS_INFO("Battery Low!!! (%fV) Landing initialized",battery_voltage);
LOW_POWER_DETECTED = true;
}
if (initCounter < initTime) {
init_acc += accel;
init_gyr += gyro;
initCounter++;
}
else if (initCounter == initTime) {
init_acc /= initTime;
gravity = init_acc.norm();
setGravity(gravity);
ROS_INFO("IMU Calibration Done! Gravity: %f", gravity);
initCounter++;
setInit(msg->header.stamp);
}
else {
processUpdate(accel, gyro, msg->header.stamp);
measureUpdate(range_al);
}
return;
}
void CGLUtil::viewerGL()
{
glMatrixMode(GL_MODELVIEW);
// load the matrix to set camera pose
glLoadMatrixf(_eimModelViewGL.data());
//rotation
Eigen::Matrix3f eimRotation;
if( btl::utility::BTL_GL == _eConvention ){
eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX()); // 3. rotate horizontally
}//mouse x-movement is the rotation around y-axis
else if( btl::utility::BTL_CV == _eConvention ) {
eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), -Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX()); // 3. rotate horizontally
}
//translation
/*_dZoom = _dZoom < 0.1? 0.1: _dZoom;
_dZoom = _dZoom > 10? 10: _dZoom;*/
//get direction N pointing from camera center to the object centroid
Eigen::Affine3f M; M = _eimModelViewGL;
Eigen::Vector3f T = M.translation();
Eigen::Matrix3f R = M.linear();
Eigen::Vector3f C = -R.transpose()*T;//camera centre
Eigen::Vector3f N = _eivCentroid - C;//from camera centre to object centroid
N = N/N.norm();//normalization
Eigen::Affine3f _eimManipulate; _eimManipulate.setIdentity();
_eimManipulate.translate( N*float(_dZoom) );//(N*(1-_dZoom)); //use camera movement toward object for zoom in/out effects
_eimManipulate.translate(_eivCentroid); // 5. translate back to the original camera pose
//_eimManipulate.scale(s); // 4. zoom in/out, never use scale to simulate camera movement, it affects z-buffer capturing. use translation instead
_eimManipulate.rotate(eimRotation); // 2. rotate vertically // 3. rotate horizontally
_eimManipulate.translate(-_eivCentroid); // 1. translate the camera center to align with object centroid*/
glMultMatrixf(_eimManipulate.data());
/*
lTranslated( _aCentroid[0], _aCentroid[1], _aCentroid[2] ); // 5. translate back to the original camera pose
_dZoom = _dZoom < 0.1? 0.1: _dZoom;
_dZoom = _dZoom > 10? 10: _dZoom;
glScaled( _dZoom, _dZoom, _dZoom ); // 4. zoom in/out,
if( btl::utility::BTL_GL == _eConvention )
glRotated ( _dXAngle, 0, 1 ,0 ); // 3. rotate horizontally
else if( btl::utility::BTL_CV == _eConvention ) //mouse x-movement is the rotation around y-axis
glRotated ( _dXAngle, 0,-1 ,0 );
glRotated ( _dYAngle, 1, 0 ,0 ); // 2. rotate vertically
glTranslated(-_aCentroid[0],-_aCentroid[1],-_aCentroid[2] ); // 1. translate the camera center to align with object centroid
*/
// light position in 3d
glLightfv(GL_LIGHT0, GL_POSITION, _aLight);
}
Eigen::Vector3f VisuilizeProcessor::VectorToEulerAngles(Eigen::Vector3f v)
{
Eigen::Vector3f normed = v / v.norm();
float x = normed(0);
float y = normed(1);
float z = normed(2);
Eigen::Vector3f result;
result(0) = std::atan2(z, x) - M_PI / 2; // yaw
result(1) = -std::atan2(y, z); // tilt
result(2) = std::atan(y / x); // roll
return result * 180 / M_PI;
}
/*void CameraPoseFinderICP::findCorresSet(unsigned level,const Mat44& cur_transform,const Mat44& last_transform_inv)
{
cudaProjectionMapFindCorrs(level,cur_transform,last_transform_inv,
_camera_params_pyramid[level],
AppParams::instance()->_icp_params.fDistThres,
AppParams::instance()->_icp_params.fNormSinThres);
}*/
bool CameraPoseFinderICP::vector6ToTransformMatrix(const Eigen::Matrix<float, 6, 1, 0, 6, 1>& x, Eigen::Matrix4f& output)
{
Eigen::Matrix3f R = Eigen::AngleAxisf(x[0], Eigen::Vector3f::UnitX()).toRotationMatrix()
*Eigen::AngleAxisf(x[1], Eigen::Vector3f::UnitY()).toRotationMatrix()
*Eigen::AngleAxisf(x[2], Eigen::Vector3f::UnitZ()).toRotationMatrix();
Eigen::Vector3f t = x.segment(3, 3);
Eigen::AngleAxisf aa(R);
float angle = aa.angle();
float d = t.norm();
if (angle > AppParams::instance()->_icp_params.fAngleShake|| d> AppParams::instance()->_icp_params.fDistShake)
{
return false;
}
output.block(0, 0, 3, 3) = R;
output.block(0, 3, 3, 1) = t;
return true;
}
// This formula comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
void
psmove_alignment_compute_objective_vector(
const Eigen::Quaternionf &q, const Eigen::Vector3f &d, const Eigen::Vector3f &s,
Eigen::Matrix<float, 3, 1> &out_f, float *out_squared_error)
{
// Computing f(q;d, s) = (q^-1 * d * q) - s
Eigen::Vector3f d_rotated = psmove_vector3f_clockwise_rotate(q, d);
Eigen::Vector3f f = d_rotated - s;
out_f(0, 0) = f.x();
out_f(1, 0) = f.y();
out_f(2, 0) = f.z();
if (out_squared_error)
{
*out_squared_error = f.norm();
}
}