bool LinearAlgebra::isLinearIndependent(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
bool result = false;
int rank;
/* TODO: test if the vectors a and b are linear independent.
Return true if they are independent, false if they are dependent.*/
Eigen::MatrixXd A(2,3);
A << a.x(), a.y(), a.z(),
b.x(), b.y(), b.z();
//std::cout<<"A:"<<A<<endl;
Eigen::FullPivLU<Eigen::MatrixXd> luA(A);
rank = luA.rank();
if (rank == 2)
result = true;
else
result = false;
return result;
}
//! 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;
}
void draw()
{
glBegin(GL_LINES);
for (EdgeCIter e = mesh.edges.begin(); e != mesh.edges.end(); e++) {
int s = 3;
Eigen::Vector3d v = e->he->vertex->position;
Eigen::Vector3d u = e->he->flip->vertex->position - v;
u.normalize();
double dl = e->length() / (double)s;
double c = 0.0;
double c2 = 0.0;
if (curvature == 0) {
c = e->he->vertex->gaussCurvature;
c2 = e->he->flip->vertex->gaussCurvature;
} else if (curvature == 1) {
c = e->he->vertex->meanCurvature;
c2 = e->he->flip->vertex->meanCurvature;
} else {
c = normalCurvatures[e->he->vertex->index];
c2 = normalCurvatures[e->he->flip->vertex->index];
}
double dc = (c2 - c) / (double)s;
for (int i = 0; i < s; i++) {
if (c < 0) glColor4f(0.0, 0.0, fabs(c), 0.6);
else glColor4f(c, 0.0, 0.0, 0.6);
glVertex3d(v.x(), v.y(), v.z());
c += dc;
if (c < 0) glColor4f(0.0, 0.0, fabs(c), 0.6);
else glColor4f(c, 0.0, 0.0, 0.6);
v += u * dl;
glVertex3d(v.x(), v.y(), v.z());
}
}
glEnd();
}
void Viewer::read()
{
for( unsigned int i = 0; !m_offset && i < readers.size(); ++i )
{
if( readers[i]->empty() ) { continue; }
Eigen::Vector3d p = readers[i]->somePoint();
m_offset = std::fabs( p.x() ) > 1000 || std::fabs( p.y() ) > 1000 || std::fabs( p.z() ) > 1000 ? p : Eigen::Vector3d( 0, 0, 0 );
std::cerr << "view-points: reader no. " << i << " scene offset (" << m_offset->transpose() << "); scene radius: " << scene_radius() << std::endl;
}
if( !m_offset ) { return; }
bool need_update = false;
for( unsigned int i = 0; i < readers.size(); ++i )
{
if( readers[i]->update( *m_offset ) > 0 ) { need_update = true; };
}
m_shutdown = true;
bool ready_to_look = true;
for( unsigned int i = 0; m_shutdown && i < readers.size(); ++i )
{
m_shutdown = m_shutdown && readers[i]->isShutdown();
ready_to_look = ready_to_look && ( readers[i]->isShutdown() || ( readers.size() > 1 && readers[i]->isStdIn() ) );
}
if( !m_cameraReader && m_cameraposition )
{
setCameraPosition( *m_cameraposition, *m_cameraorientation );
m_cameraposition.reset();
m_cameraorientation.reset();
}
else if( m_cameraReader )
{
Eigen::Vector3d position = m_cameraReader->position();
Eigen::Vector3d orientation = m_cameraReader->orientation();
if( !m_cameraposition || !m_cameraposition->isApprox( position ) || !m_cameraorientation->isApprox( orientation ) )
{
m_cameraposition = position;
m_cameraorientation = orientation;
setCameraPosition( position, orientation );
}
}
else if( readers[0]->m_extents && readers[0]->m_num_points > 0 && ( m_shutdown || ready_to_look || readers[0]->m_num_points >= readers[0]->size / 10 ) )
{
QVector3D min( readers[0]->m_extents->min().x(), readers[0]->m_extents->min().y(), readers[0]->m_extents->min().z() );
QVector3D max( readers[0]->m_extents->max().x(), readers[0]->m_extents->max().y(), readers[0]->m_extents->max().z() );
updateView( min, max );
if( !m_cameraFixed && !m_lookAt )
{
m_lookAt = true;
lookAtCenter();
}
}
if( need_update ) { update(); }
if( m_shutdown && m_exit_on_end_of_input ) { shutdown(); }
}
//! Calculate the Cartesian position from geodetic coordinates.
Eigen::Vector3d convertGeodeticToCartesianCoordinates( const Eigen::Vector3d geodeticCoordinates,
const double equatorialRadius,
const double flattening )
{
// Pre-compute values for efficiency.
const double sineLatitude = std::sin( geodeticCoordinates.y( ) );
const double centerOffset = equatorialRadius /
std::sqrt( 1.0 - flattening * ( 2.0 - flattening ) * sineLatitude * sineLatitude );
// Calculate Cartesian coordinates, Montenbruck and Gill (2000), Eq. (5.82).
Eigen::Vector3d cartesianCoordinates = Eigen::Vector3d::Zero( );
cartesianCoordinates.x( ) =
( centerOffset + geodeticCoordinates.x( ) ) *
std::cos( geodeticCoordinates.y( ) ) * std::cos( geodeticCoordinates.z( ) );
cartesianCoordinates.y( ) = cartesianCoordinates.x( ) * std::tan( geodeticCoordinates.z( ) );
cartesianCoordinates.z( ) = ( ( 1.0 - flattening ) * ( 1.0 - flattening ) * centerOffset +
geodeticCoordinates.x( ) ) * sineLatitude;
// Return Cartesian coordinates.
return cartesianCoordinates;
}
Eigen::Vector4d Face::plane() const
{
Eigen::Vector3d a = he->vertex->position;
Eigen::Vector3d n = normal();
n.normalize();
Eigen::Vector4d p;
p << n.x(), n.y(), n.z(), -n.dot(a);
return p;
}
Hole HoleFinderPrivate::reduceHoles(const QVector<Hole *> &holeVec)
{
// Average the positions, weighted by volume
Eigen::Vector3d center (0.0, 0.0, 0.0);
double denom = 0.0;
foreach (Hole *hole, holeVec) {
const double volume = hole->volume();
center += hole->center * volume;
denom += volume;
}
center /= denom;
// Wrap center into the unit cell
this->cartToFrac(¢er);
if ((center.x() = fmod(center.x(), 1.0)) < 0) ++center.x();
if ((center.y() = fmod(center.y(), 1.0)) < 0) ++center.y();
if ((center.z() = fmod(center.z(), 1.0)) < 0) ++center.z();
this->fracToCart(¢er);
return Hole(center, 0.0); // radius is set during resizeHoles();
}
void OrientationCell::rotateSurf(Eigen::Matrix3d& rotation, Eigen::Vector3d& center, PlanCloud& planCloud)
{
rotate(rotation, planCloud.T());
rotate(rotation, planCloud.B());
rotate(rotation, planCloud.N());
*(planCloud.frame()) << planCloud.T(), planCloud.B(), planCloud.N();
Eigen::Vector3d OgOi(planCloud.origin()->x() - center.x(),
planCloud.origin()->y() - center.y(),
planCloud.origin()->z() - center.z());
OgOi = rotation*OgOi;
*(planCloud.origin()) << OgOi.x(), OgOi.y(), OgOi.z();
}
inline bool GradientIsEffectiveFlat(const Eigen::Vector3d& gradient) const
{
// A gradient is at a local maxima if the absolute value of all components (x,y,z) are less than 1/2 SDF resolution
double half_resolution = GetResolution() * 0.5;
if (fabs(gradient.x()) <= half_resolution && fabs(gradient.y()) <= half_resolution && fabs(gradient.z()) <= half_resolution)
{
return true;
}
else
{
return false;
}
}
bool addConeToCollisionModel(const std::string &armName, double length, double radius)
{
ros::NodeHandle nh("~") ;
ros::service::waitForService("/environment_server/set_planning_scene_diff");
ros::ServiceClient get_planning_scene_client =
nh.serviceClient<arm_navigation_msgs::GetPlanningScene>("/environment_server/set_planning_scene_diff");
arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
arm_navigation_msgs::AttachedCollisionObject att_object;
att_object.link_name = armName + "_gripper";
att_object.object.id = "attached_cone";
att_object.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
att_object.object.header.frame_id = "base_link";
att_object.object.header.stamp = ros::Time::now();
Eigen::Vector3d p = robot_helpers::getPose(armName).inverse().translation() ;
arm_navigation_msgs::Shape object;
shapes::Mesh mesh ;
makeSolidCone(mesh, radius, length, 10, 20) ;
if(!planning_environment::constructObjectMsg(&mesh, object)) {
ROS_WARN_STREAM("Object construction fails");
}
geometry_msgs::Pose pose;
pose.position.x = p.x();
pose.position.y = p.y();
pose.position.z = p.z() ;
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = 0;
pose.orientation.w = 1;
att_object.object.shapes.push_back(object);
att_object.object.poses.push_back(pose);
planning_scene_req.planning_scene_diff.attached_collision_objects.push_back(att_object);
if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) return false;
return true ;
}
//==============================================================================
void OdeMesh::fillArrays(const aiScene* scene, const Eigen::Vector3d& scale)
{
mVertices.clear();
mNormals.clear();
mIndices.clear();
// Cound the total numbers of vertices and indices.
auto mNumVertices = 0u;
auto mNumIndices = 0u;
for (auto i = 0u; i < scene->mNumMeshes; ++i)
{
const auto mesh = scene->mMeshes[i];
mNumVertices += mesh->mNumVertices;
mNumIndices += mesh->mNumFaces;
}
mNumVertices *= 3u;
mNumIndices *= 3u;
// The number of indices of each face is always 3 because we use the assimp
// option `aiProcess_Triangulate` when loading meshes.
mVertices.resize(mNumVertices);
mNormals.resize(mNumVertices);
mIndices.resize(mNumIndices);
auto vertexIndex = 0u;
auto indexIndex = 0u;
for (auto i = 0u; i < scene->mNumMeshes; ++i)
{
const auto mesh = scene->mMeshes[i];
for (auto j = 0u; j < mesh->mNumVertices; ++j)
{
mVertices[vertexIndex] = mesh->mVertices[j].x * scale.x();
mNormals[vertexIndex++] = mesh->mNormals[j].x;
mVertices[vertexIndex] = mesh->mVertices[j].y * scale.y();
mNormals[vertexIndex++] = mesh->mNormals[j].y;
mVertices[vertexIndex] = mesh->mVertices[j].z * scale.z();
mNormals[vertexIndex++] = mesh->mNormals[j].z;
}
for (auto j = 0u; j < mesh->mNumFaces; ++j)
{
mIndices[indexIndex++] = mesh->mFaces[j].mIndices[0];
mIndices[indexIndex++] = mesh->mFaces[j].mIndices[1];
mIndices[indexIndex++] = mesh->mFaces[j].mIndices[2];
}
}
}
int main(int argc, char** argv) {
Eigen::Matrix3d m;
m<<3, 1, 2,
1, 3, 7,
2, 7, 4;
Eigen::EigenSolver<Eigen::Matrix3d> solver( m );
for( int i = 0 ; i < 3 ; ++i ) {
const double ev = solver.eigenvalues()( i,0 ).real(); //固有値
const Eigen::Vector3d v = solver.eigenvectors().col(i).real(); //固有ベクトル
std::cerr<<i<<" : "<<ev<<" ( "<<v.x()<<", "<<v.y()<<", "<<v.z()<<")"<<std::endl;
}
return 0;
}
static void subject_publish_callback(const ViconDriver::Subject &subject)
{
if(!running)
return;
static std::map<std::string, ros::Publisher> vicon_publishers;
std::map<std::string, ros::Publisher>::iterator it;
it = vicon_publishers.find(subject.name);
if(it == vicon_publishers.end())
{
ros::Publisher pub = nh->advertise<vicon::Subject>(subject.name, 10);
it = vicon_publishers.insert(std::make_pair(subject.name, pub)).first;
}
if(loadCalib(subject.name))
{
Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
current_pose.translate(Eigen::Vector3d(subject.translation));
current_pose.rotate(Eigen::Quaterniond(subject.rotation));
current_pose = current_pose * calib_pose[subject.name];
const Eigen::Vector3d position(current_pose.translation());
const Eigen::Quaterniond rotation(current_pose.rotation());
vicon::Subject subject_ros;
subject_ros.header.seq = subject.frame_number;
subject_ros.header.stamp = ros::Time(subject.time_usec / 1e6);
subject_ros.header.frame_id = "/vicon";
subject_ros.name = subject.name;
subject_ros.occluded = subject.occluded;
subject_ros.position.x = position.x();
subject_ros.position.y = position.y();
subject_ros.position.z = position.z();
subject_ros.orientation.x = rotation.x();
subject_ros.orientation.y = rotation.y();
subject_ros.orientation.z = rotation.z();
subject_ros.orientation.w = rotation.w();
subject_ros.markers.resize(subject.markers.size());
for(size_t i = 0; i < subject_ros.markers.size(); i++)
{
subject_ros.markers[i].name = subject.markers[i].name;
subject_ros.markers[i].subject_name = subject.markers[i].subject_name;
subject_ros.markers[i].position.x = subject.markers[i].translation[0];
subject_ros.markers[i].position.y = subject.markers[i].translation[1];
subject_ros.markers[i].position.z = subject.markers[i].translation[2];
subject_ros.markers[i].occluded = subject.markers[i].occluded;
}
it->second.publish(subject_ros);
}
}
vector<cv::Point2f> FeatureTracker::undistortedPoints()
{
vector<cv::Point2f> un_pts;
//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
}
return un_pts;
}
inline void BasisSet::pointD5(BasisSet *set, const Eigen::Vector3d &delta,
const double &dr2, int basis,
Eigen::MatrixXd &out)
{
// D type orbitals have six components and each component has a different
// independent MO weighting. Many things can be cached to save time though
double d0 = 0.0, d1p = 0.0, d1n = 0.0, d2p = 0.0, d2n = 0.0;
// Now iterate through the D type GTOs and sum their contributions
unsigned int cIndex = set->m_cIndices[basis];
for (unsigned int i = set->m_gtoIndices[basis];
i < set->m_gtoIndices[basis+1]; ++i) {
// Calculate the common factor
double tmpGTO = exp(-set->m_gtoA[i] * dr2);
d0 += set->m_gtoCN[cIndex++] * tmpGTO;
d1p += set->m_gtoCN[cIndex++] * tmpGTO;
d1n += set->m_gtoCN[cIndex++] * tmpGTO;
d2p += set->m_gtoCN[cIndex++] * tmpGTO;
d2n += set->m_gtoCN[cIndex++] * tmpGTO;
}
// Calculate the prefactors
double xx = delta.x() * delta.x();
double yy = delta.y() * delta.y();
double zz = delta.z() * delta.z();
double xy = delta.x() * delta.y();
double xz = delta.x() * delta.z();
double yz = delta.y() * delta.z();
// Save values to the matrix
int baseIndex = set->m_moIndices[basis];
out.coeffRef(baseIndex , 0) = (zz - dr2) * d0;
out.coeffRef(baseIndex+1, 0) = xz * d1p;
out.coeffRef(baseIndex+2, 0) = yz * d1n;
out.coeffRef(baseIndex+3, 0) = (xx - yy) * d2p;
out.coeffRef(baseIndex+4, 0) = xy * d2n;
}
bool VectorVisualization::visualize(const grid_map::GridMap& map)
{
if (!isActive()) return true;
for (const auto& type : types_) {
if (!map.exists(type)) {
ROS_WARN_STREAM(
"VectorVisualization::visualize: No grid map layer with name '" << type << "' found.");
return false;
}
}
// Set marker info.
marker_.header.frame_id = map.getFrameId();
marker_.header.stamp.fromNSec(map.getTimestamp());
// Clear points.
marker_.points.clear();
marker_.colors.clear();
for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
{
if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue;
geometry_msgs::Vector3 vector;
vector.x = map.at(types_[0], *iterator); // FIXME(cfo): segfaults when types is not set
vector.y = map.at(types_[1], *iterator);
vector.z = map.at(types_[2], *iterator);
Eigen::Vector3d position;
map.getPosition3(positionLayer_, *iterator, position);
geometry_msgs::Point startPoint;
startPoint.x = position.x();
startPoint.y = position.y();
startPoint.z = position.z();
marker_.points.push_back(startPoint);
geometry_msgs::Point endPoint;
endPoint.x = startPoint.x + scale_ * vector.x;
endPoint.y = startPoint.y + scale_ * vector.y;
endPoint.z = startPoint.z + scale_ * vector.z;
marker_.points.push_back(endPoint);
marker_.colors.push_back(color_); // Each vertex needs a color.
marker_.colors.push_back(color_);
}
publisher_.publish(marker_);
return true;
}
bool Reader::updatePoint( const Eigen::Vector3d& offset )
{
if( !m_point ) { return false; } // is it safe to do it without locking the mutex?
boost::mutex::scoped_lock lock( m_mutex );
if( !updated_ ) { return false; }
m_translation = QVector3D( m_point->x(), m_point->y(), m_point->z() );
m_offset = QVector3D( offset.x(), offset.y(), offset.z() );
if( m_orientation )
{
const Eigen::Quaterniond& q = snark::rotation_matrix( *m_orientation ).quaternion();
m_quaternion = QQuaternion( q.w(), q.x(), q.y(), q.z() );
}
updated_ = false;
return true;
}
void collision_detection::StaticDistanceField::determineCollisionPoints(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}
void collision_detection::StaticDistanceField::getCellGradient(
int cell_id,
Eigen::Vector3d& gradient) const
{
if (cell_id + this->stride2_ + this->stride1_ + 1 >= num_cells_total_ ||
cell_id - this->stride2_ - this->stride1_ - 1 < 0)
{
gradient = Eigen::Vector3d(1,0,0);
return;
}
gradient.x() = (getCell(cell_id + stride1_).distance_ - getCell(cell_id - stride1_).distance_) * inv_twice_resolution_;
gradient.y() = (getCell(cell_id + stride2_).distance_ - getCell(cell_id - stride2_).distance_) * inv_twice_resolution_;
gradient.z() = (getCell(cell_id + 1).distance_ - getCell(cell_id - 1).distance_) * inv_twice_resolution_;
}
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);
}
void distance_field::findInternalPointsConvex(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}
Eigen::Vector2d KinematicUtil::calculate_kinematic_robot_angle(const KRobot& robot, int sup) {
JointIds supId = (sup == SupportLeg::left? JointIds::LFootEndpoint: JointIds::RFootEndpoint);
Eigen::Affine3d suppport_transform = robot[supId].get_chain_matrix_inverse();
Eigen::Vector3d angle = suppport_transform.linear().col(2);//.normalized();
if(angle == Eigen::Vector3d::UnitZ()) {
return Eigen::Vector2d::Zero();
}
//TODO I think I still need some normalisation here, but for now this should be sufficient. I only expect a rotation on one axis,
// so the normalization should not be crucial required.
double x_part = Vector2d{angle.y(), angle.z()}.normalized().x();
double y_part = Vector2d{angle.x(), angle.z()}.normalized().x();
// I don't know how to fix the scaling for the second angle on the x-axis, but it's not so important, I'll leave this as TODO
// TODO fix the scaling, so that we can handle both axes correctly and simultaneously
return angle.z() < 0.0 ? Eigen::Vector2d{-asin(x_part), pi - asin(y_part)}
: Eigen::Vector2d{-asin(x_part), asin(y_part)};
}
point( const snark::las::point< 1 >& p, const Eigen::Vector3d& factor, const Eigen::Vector3d& offset )
: coordinates( Eigen::Vector3d( factor.x() * p.coordinates.x()
, factor.y() * p.coordinates.y()
, factor.z() * p.coordinates.z() ) + offset )
, intensity( p.intensity() )
, return_number( p.returns().number )
, number_of_returns( p.returns().size )
, scan_direction( p.returns().scan_direction )
, edge_of_flight_line( p.returns().edge_of_flight_line )
, classification( p.classification() )
, scan_angle( p.scan_angle() )
, user_data( p.user_data() )
, point_source_id( p.point_source_id() )
, gps_time( p.gps_time() )
{
}
Eigen::Vector3d pose3Dto2D (geometry_msgs::Point const & translation,
geometry_msgs::Quaternion const & orientation)
{
// Can somebody please explain to me why Eigen has chosen to store
// the parameters in (x,y,z,w) order, but provide a ctor in
// (w,x,y,z) order? What have they been smoking?
//
Eigen::Quaternion<double> const rot (orientation.w,
orientation.x,
orientation.y,
orientation.z);
Eigen::Vector3d ux;
ux << 1.0, 0.0, 0.0;
ux = rot._transformVector (ux);
Eigen::Vector3d pose (translation.x, translation.y, atan2 (ux.y(), ux.x()));
return pose;
}
void feedbackMaster()
{
// WEIRD MAPPING!!!
phantom_omni::OmniFeedback force_feedback;
force_feedback.force.x=resulting_force.y();
force_feedback.force.y=resulting_force.z();
force_feedback.force.z=resulting_force.x();
force_out.publish(force_feedback);
// geometry_msgs::Twist twist_msg_resulting_force;
// twist_msg_resulting_force.linear.x= resulting_force.y();
// twist_msg_resulting_force.linear.y=resulting_force.x();
// twist_msg_resulting_force.linear.z=resulting_force.z();
// repulsive_force_out.publish(twist_msg_resulting_force);
}
void vision_speed_estimate(uint64_t usec, Eigen::Vector3d &v)
{
mavlink::common::msg::VISION_SPEED_ESTIMATE vs{};
vs.usec = usec;
// [[[cog:
// for f in "xyz":
// cog.outl("vs.%s = v.%s();" % (f, f))
// ]]]
vs.x = v.x();
vs.y = v.y();
vs.z = v.z();
// [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb)
UAS_FCU(m_uas)->send_message_ignore_drop(vs);
}
TEST (PCL, WarpPointRigid6DDouble)
{
WarpPointRigid6D<PointXYZ, PointXYZ, double> warp;
Eigen::Quaterniond q (0.4455, 0.9217, 0.3382, 0.3656);
q.normalize ();
Eigen::Vector3d t (0.82550, 0.11697, 0.44864);
Eigen::VectorXd p (6);
p[0] = t.x (); p[1] = t.y (); p[2] = t.z (); p[3] = q.x (); p[4] = q.y (); p[5] = q.z ();
warp.setParam (p);
PointXYZ pin (1, 2, 3), pout;
warp.warpPoint (pin, pout);
EXPECT_NEAR (pout.x, 4.15963, 1e-5);
EXPECT_NEAR (pout.y, -1.51363, 1e-5);
EXPECT_NEAR (pout.z, 0.922648, 1e-5);
}
bool planArmToPose(const string &armName, const Eigen::Vector3d &pos, const Eigen::Quaterniond &q, trajectory_msgs::JointTrajectory &traj, arm_navigation_msgs::SimplePoseConstraint *poseConstraint)
{
clopema_arm_navigation::ClopemaMotionPlan mp;
mp.request.motion_plan_req.group_name = armName + "_arm" ;
mp.request.motion_plan_req.allowed_planning_time = ros::Duration(5.0);
if ( !getRobotState(mp.request.motion_plan_req.start_state) ) return false ;
arm_navigation_msgs::SimplePoseConstraint desired_pose;
desired_pose.header.frame_id = "base_link";
desired_pose.header.stamp = ros::Time::now();
desired_pose.link_name = armName + "_ee";
desired_pose.pose.position.x = pos.x() ;
desired_pose.pose.position.y = pos.y() ;
desired_pose.pose.position.z = pos.z() ;
desired_pose.pose.orientation.x = q.x() ;
desired_pose.pose.orientation.y = q.y() ;
desired_pose.pose.orientation.z = q.z() ;
desired_pose.pose.orientation.w = q.w() ;
if ( poseConstraint )
{
desired_pose.absolute_position_tolerance = poseConstraint->absolute_position_tolerance ;
desired_pose.absolute_roll_tolerance = poseConstraint->absolute_roll_tolerance ;
desired_pose.absolute_pitch_tolerance = poseConstraint->absolute_pitch_tolerance ;
desired_pose.absolute_yaw_tolerance = poseConstraint->absolute_yaw_tolerance ;
}
else
{
desired_pose.absolute_position_tolerance.x = desired_pose.absolute_position_tolerance.y = desired_pose.absolute_position_tolerance.z = 0.02;
desired_pose.absolute_roll_tolerance = desired_pose.absolute_pitch_tolerance = desired_pose.absolute_yaw_tolerance = 0.04;
}
poseToClopemaMotionPlan(mp, desired_pose);
if (!plan(mp)) return false;
traj = mp.response.joint_trajectory ;
if ( traj.points.size() > 2 ) return true ;
return false;
}
bool VectorVisualization::visualize(const grid_map::GridMap& map)
{
if (markerPublisher_.getNumSubscribers () < 1) return true;
// Set marker info.
marker_.header.frame_id = map.getFrameId();
marker_.header.stamp.fromNSec(map.getTimestamp());
// Clear points.
marker_.points.clear();
marker_.colors.clear();
for (grid_map_lib::GridMapIterator iterator(map); !iterator.isPassedEnd(); ++iterator)
{
if (!map.isValid(*iterator) || !map.isValid(*iterator, types_)) continue;
geometry_msgs::Vector3 vector;
vector.x = map.at(types_[0], *iterator);
vector.y = map.at(types_[1], *iterator);
vector.z = map.at(types_[2], *iterator);
Eigen::Vector3d position;
map.getPosition3d(positionType_, *iterator, position);
geometry_msgs::Point startPoint;
startPoint.x = position.x();
startPoint.y = position.y();
startPoint.z = position.z();
marker_.points.push_back(startPoint);
geometry_msgs::Point endPoint;
endPoint.x = startPoint.x + scale_ * vector.x;
endPoint.y = startPoint.y + scale_ * vector.y;
endPoint.z = startPoint.z + scale_ * vector.z;
marker_.points.push_back(endPoint);
marker_.colors.push_back(color_); // Each vertex needs a color.
marker_.colors.push_back(color_);
}
markerPublisher_.publish(marker_);
return true;
}
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);
}
}
}
}
}
}