PlaneExt::PlaneExt(but_plane_detector::Plane<float> plane) : but_plane_detector::Plane<float> (0.0, 0.0, 0.0, 0.0), planeCoefficients(new pcl::ModelCoefficients()) { a = plane.a; b = plane.b; c = plane.c; d = plane.d; norm = plane.norm; // Create a quaternion for rotation into XY plane Eigen::Vector3f current(a, b, c); Eigen::Vector3f target(0.0, 0.0, 1.0); Eigen::Quaternion<float> q; q.setFromTwoVectors(current, target); planeTransXY = q.toRotationMatrix(); planeShift = -d; color.r = abs(a) / 2.0 + 0.2; color.g = abs(b) / 2.0 + 0.2; color.b = abs(d) / 5.0; color.a = 1.0; // Plane coefficients pre-calculation... planeCoefficients->values.push_back(a); planeCoefficients->values.push_back(b); planeCoefficients->values.push_back(c); planeCoefficients->values.push_back(d); }
bool testRawDataAcces() { bool passed = true; Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0}; Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data()); SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(), raw, Constants<Scalar>::epsilon()); SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(), raw.data()); Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3; SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(), map_of_const_rxso3.quaternion().coeffs().eval()); Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0}; Eigen::Map<RxSO3Type> map_of_rxso3(raw.data()); Eigen::Quaternion<Scalar> quat; quat.coeffs() = raw2; map_of_rxso3.setQuaternion(quat); SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2, Constants<Scalar>::epsilon()); SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(), raw.data()); SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(), quat.coeffs().data()); Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3; SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(), map_of_rxso3.quaternion().coeffs().eval()); RxSO3Type const const_so3(quat); for (int i = 0; i < 4; ++i) { SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]); } RxSO3Type so3(quat); for (int i = 0; i < 4; ++i) { so3.data()[i] = raw[i]; } for (int i = 0; i < 4; ++i) { SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]); } for (int i = 0; i < 10; ++i) { Matrix3<Scalar> M = Matrix3<Scalar>::Random(); for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) { Matrix3<Scalar> R = makeRotationMatrix(M); Matrix3<Scalar> sR = scale * R; SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR), "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R); Matrix3<Scalar> sR_cols_swapped; sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2); SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped), "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R); } } return passed; }
// returns the local R,t in nd0 that produces nd1 // NOTE: returns a postfix rotation; should return a prefix void transformN2N(Eigen::Matrix<double,4,1> &trans, Eigen::Quaternion<double> &qr, Node &nd0, Node &nd1) { Matrix<double,3,4> tfm; Quaterniond q0,q1; q0 = nd0.qrot; transformW2F(tfm,nd0.trans,q0); trans.head(3) = tfm*nd1.trans; trans(3) = 1.0; q1 = nd1.qrot; qr = q0.inverse()*q1; qr.normalize(); if (qr.w() < 0) qr.coeffs() = -qr.coeffs(); }
Eigen::Quaternion<T> naive_slerp(const Eigen::Quaternion<T>& input, const Eigen::Quaternion<T>& target, T amt){ Eigen::Quaternion<T> result; if (amt==T(0)) { return input; } else if (amt==T(1)) { return target; } int bflip = 0; T dot_prod = input.dot(target); T a, b; //clamp dot_prod = (dot_prod < -1) ? -1 : ((dot_prod > 1) ? 1 : dot_prod); // if B is on opposite hemisphere from A, use -B instead if (dot_prod < 0.0) { dot_prod = -dot_prod; bflip = 1; } T cos_angle = acos(dot_prod); if(ABS(cos_angle) > QUAT_EPSILON) { T sine = sin(cos_angle); T inv_sine = 1./sine; a = sin(cos_angle*(1.-amt)) * inv_sine; b = sin(cos_angle*amt) * inv_sine; if (bflip) { b = -b; } } else { // nearly the same; // approximate without trigonometry a = amt; b = 1.-amt; } result.w = a*input.w + b*target.w; result.x = a*input.x + b*target.x; result.y = a*input.y + b*target.y; result.z = a*input.z + b*target.z; result.normalize(); return result; }
inline static void setFromVectors(Rotation_& rot, const Eigen::Matrix<PrimType_, 3, 1>& v1, const Eigen::Matrix<PrimType_, 3, 1>& v2) { KINDR_ASSERT_TRUE(std::runtime_error, v1.norm()*v2.norm() != static_cast<PrimType_>(0.0), "At least one vector has zero length."); Eigen::Quaternion<PrimType_> eigenQuat; eigenQuat.setFromTwoVectors(v1, v2); rot = kindr::rotations::eigen_impl::RotationQuaternion<PrimType_, Usage_>(eigenQuat); // // const PrimType_ angle = acos(v1.dot(v2)/temp); // const PrimType_ tol = 1e-3; // // if(0 <= angle && angle < tol) { // rot.setIdentity(); // } else if(M_PI - tol < angle && angle < M_PI + tol) { // rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, 1, 0, 0); // } else { // const Eigen::Matrix<PrimType_, 3, 1> axis = (v1.cross(v2)).normalized(); // rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, axis); // } }
visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){ //------- Compute Principal Componets for Marker publishing //Get the principal components and the quaternion Eigen::Matrix3f evecs; Eigen::Vector3f evals; //pcl::eigen33 (covariance_matrix, evecs, evals); Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix); evecs = es.eigenvectors().real(); evals = es.eigenvalues().real(); Eigen::Matrix3f rotation; rotation.row (0) = evecs.col (0); rotation.row (1) = evecs.col (1); rotation.row (2) = rotation.row (0).cross (rotation.row (1)); rotation.transposeInPlace (); Eigen::Quaternion<float> qt (rotation); qt.normalize (); //Publish Marker for cluster visualization_msgs::Marker marker; marker.header.frame_id = base_frame_; marker.header.stamp = ros::Time().now(); marker.ns = "Perception"; marker.action = visualization_msgs::Marker::ADD; marker.id = marker_id; marker.lifetime = ros::Duration(1); //centroid position marker.type = visualization_msgs::Marker::SPHERE; marker.pose.position.x = centroid[0]; marker.pose.position.y = centroid[1]; marker.pose.position.z = centroid[2]; marker.pose.orientation.x = qt.x(); marker.pose.orientation.y = qt.y(); marker.pose.orientation.z = qt.z(); marker.pose.orientation.w = qt.w(); //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl; marker.scale.x = sqrt(evals(0)) * 4; marker.scale.y = sqrt(evals(1)) * 4; marker.scale.z = sqrt(evals(2)) * 4; //give it some color! marker.color.a = 0.75; satToRGB(marker.color.r, marker.color.g, marker.color.b); //std::cout << "marker being published" << std::endl; return marker; }
IGL_INLINE void igl::two_axis_valuator_fixed_up( const int w, const int h, const double speed, const Eigen::Quaternion<Scalardown_quat> & down_quat, const int down_x, const int down_y, const int mouse_x, const int mouse_y, Eigen::Quaternion<Scalarquat> & quat) { using namespace Eigen; Matrix<Scalarquat,3,1> axis(0,1,0); quat = down_quat * Quaternion<Scalarquat>( AngleAxis<Scalarquat>( PI*((Scalarquat)(mouse_x-down_x))/(Scalarquat)w*speed/2.0, axis.normalized())); quat.normalize(); { Matrix<Scalarquat,3,1> axis(1,0,0); if(axis.norm() != 0) { quat = Quaternion<Scalarquat>( AngleAxis<Scalarquat>( PI*(mouse_y-down_y)/(Scalarquat)h*speed/2.0, axis.normalized())) * quat; quat.normalize(); } } }
SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i Matrix<3> iniOrientationEKF; iniOrientationEKF = tag::quaternionToMatrix(attivec); Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in 0, -1, 0, // a world frame which pointing downward. 0, 0, -1); SE3<> camWorld = SE3<>(); Matrix<3> rotation; if (tracker_->attitude_get) rotation = iniOrientationEKF; // else rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric) Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation(); camWorld.get_translation()[0] = 0.0;//twc[0]; //twc camWorld.get_translation()[1] = 0.0;//twc[1]; camWorld.get_translation()[2] = twc[2]; camWorld = camWorld.inverse();//Tcw cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl; // cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl; return camWorld; }
void HLManager::onVTTwist(const geometry_msgs::TwistStamped::ConstPtr& twist) { if (mode == circle || mode == vtManual) { //\todo Generalize this 0.1 with Ts. s +=twist->twist.linear.x*0.1; //Circle if (s>=2*circleRadius*M_PI) s=s-2*circleRadius*M_PI; else if (s<0) s=2*circleRadius*M_PI-s; double xRabbit = point.point.x + circleRadius*cos(s/circleRadius); double yRabbit = point.point.y + circleRadius*sin(s/circleRadius); double gammaRabbit=labust::math::wrapRad(s/circleRadius)+M_PI/2; tf::Transform transform; transform.setOrigin(tf::Vector3(xRabbit, yRabbit, 0)); Eigen::Quaternion<float> q; labust::tools::quaternionFromEulerZYX(0,0,gammaRabbit, q); transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w())); broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "serret_frenet_frame")); auv_msgs::NavStsPtr msg(new auv_msgs::NavSts()); msg->position.north = xRabbit; msg->position.east = yRabbit; msg->body_velocity.x = twist->twist.linear.x; msg->orientation.yaw = gammaRabbit; sfPub.publish(msg); } }
void HLManager::start() { ros::Rate rate(10); while (nh.ok()) { if (fixValidated) { tf::Transform transform; transform.setOrigin(tf::Vector3(originLon, originLat, 0)); transform.setRotation(tf::createQuaternionFromRPY(0,0,0)); broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/worldLatLon", "/world")); transform.setOrigin(tf::Vector3(0, 0, 0)); Eigen::Quaternion<float> q; labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q); transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w())); broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "local")); } this->safetyTest(); this->step(); hlMessagePub.publish(hlDiagnostics); rate.sleep(); ros::spinOnce(); } }
bool CloudMatcher::match(modular_cloud_matcher::MatchClouds::Request& req, modular_cloud_matcher::MatchClouds::Response& res) { // get and check reference size_t referenceGoodCount; DP referenceCloud(rosMsgToPointMatcherCloud(req.reference, referenceGoodCount)); const unsigned referencePointCount(req.reference.width * req.reference.height); const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount)); if (referenceGoodCount == 0) { ROS_ERROR("I found no good points in the reference cloud"); return false; } if (referenceGoodRatio < 0.5) { ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")"); } // get and check reading size_t readingGoodCount; DP readingCloud(rosMsgToPointMatcherCloud(req.readings, readingGoodCount)); const unsigned readingPointCount(req.readings.width * req.readings.height); const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount)); if (readingGoodCount == 0) { ROS_ERROR("I found no good points in the reading cloud"); return false; } if (readingGoodRatio < 0.5) { ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")"); } // call icp TP transform; try { transform = icp(readingCloud, referenceCloud); ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl); } catch (PM::ConvergenceError error) { ROS_ERROR_STREAM("ICP failed to converge: " << error.what()); return false; } // fill return value res.transform.translation.x = transform.coeff(0,3); res.transform.translation.y = transform.coeff(1,3); res.transform.translation.z = transform.coeff(2,3); const Eigen::Quaternion<Scalar> quat(Matrix3(transform.block(0,0,3,3))); res.transform.rotation.x = quat.x(); res.transform.rotation.y = quat.y(); res.transform.rotation.z = quat.z(); res.transform.rotation.w = quat.w(); return true; }
void quaternion2vector(const Eigen::Quaternion<T_qt> &quat_In, std::vector<T_vec> &quat_Vec ) { quat_Vec.resize(4,T_vec(0.0)); quat_Vec[0] = T_vec( quat_In.w() ); quat_Vec[1] = T_vec( quat_In.x() ); quat_Vec[2] = T_vec( quat_In.y() ); quat_Vec[3] = T_vec( quat_In.z() ); };
IGL_INLINE bool igl::snap_to_canonical_view_quat( const Eigen::Quaternion<Scalarq> & q, const double threshold, Eigen::Quaternion<Scalars> & s) { return snap_to_canonical_view_quat<Scalars>( q.coeffs().data(),threshold,s.coeffs().data()); }
void SetTwistHandler::handleSimulation(){ // called when the main script calls: simHandleModule if(!_initialized){ _initialize(); } Eigen::Quaternion < simFloat > orientation; //(x,y,z,w) Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x, (simFloat)_twistCommands.twist.linear.y, (simFloat)_twistCommands.twist.linear.z); Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x, (simFloat)_twistCommands.twist.angular.y, (simFloat)_twistCommands.twist.angular.z); // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame. if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){ linVelocity = orientation*linVelocity; angVelocity = orientation*angVelocity; } else { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl; ConsoleHandler::printInConsole(ss); } simResetDynamicObject(_associatedObjectID); //Set object velocity if (_isStatic){ Eigen::Matrix<simFloat, 3, 1> position; simGetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat timeStep = simGetSimulationTimeStep(); position = position + timeStep*linVelocity; simSetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat angle = timeStep*angVelocity.norm(); if(angle > 1e-6){ orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation; } simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()); } else { // Apply the linear velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } // Apply the angular velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } } }
void GetTwistHandler::handleSimulation(){ // called when the main script calls: simHandleModule if(!_initialized){ _initialize(); } ros::Time now = ros::Time::now(); const simFloat currentSimulationTime = simGetSimulationTime(); if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){ Eigen::Quaternion< simFloat > orientation; //(x,y,z,w) Eigen::Matrix<simFloat, 3, 1> linVelocity; Eigen::Matrix<simFloat, 3, 1> angVelocity; bool error = false; // Get object velocity. If the object is not static simGetVelocity is more accurate. if (_isStatic){ error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1; } else { error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1; } // Get object orientation error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1; if(!error){ linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame // Fill the status msg geometry_msgs::TwistStamped msg; msg.twist.linear.x = linVelocity[0]; msg.twist.linear.y = linVelocity[1]; msg.twist.linear.z = linVelocity[2]; msg.twist.angular.x = angVelocity[0]; msg.twist.angular.y = angVelocity[1]; msg.twist.angular.z = angVelocity[2]; msg.header.stamp = now; _pub.publish(msg); _lastPublishedObjTwistTime = currentSimulationTime; } else { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;; ConsoleHandler::printInConsole(ss); } } }
void YouBot::updateWheels(const ros::TimerEvent& e) { try { if(q_base_.rows() != robot_->getMacroManipulatorDOF()) return; if(tau_base_.rows() != robot_->getMacroManipulatorDOF()) return; if(!gazebo_interface_wheel_->subscribed()) return; Eigen::VectorXd q = gazebo_interface_wheel_->getJointStates(); robot_->updateWheel(q); Eigen::Vector3d base_pos; base_pos << q_base_[0], q_base_[1], 0.0; Eigen::Quaternion<double> base_ori; base_ori.x() = 0.0; base_ori.y() = 0.0; base_ori.z() = sin(0.5 * q_base_[2]); base_ori.w() = cos(0.5 * q_base_[2]); robot_->updateBase(base_pos, base_ori); Eigen::VectorXd v_base; controller_->computeBaseVelocityFromTorque(tau_base_, v_base, 3); Eigen::VectorXd v_wheel; controller_->computeWheelVelocityFromBaseVelocity(v_base, v_wheel); Eigen::VectorXd q_wheel_d = robot_->getMobility()->update_rate * v_wheel; std::vector<Eigen::Quaternion<double> > quat_d; for(unsigned int i = 0; i < q_wheel_d.rows(); ++i) { double rad = q_wheel_d[i]; Eigen::Quaternion<double> quat; quat.x() = 0.0; quat.y() = sin(0.5 * rad); quat.z() = 0.0; quat.w() = cos(0.5 * rad); quat_d.push_back(quat); } gazebo_interface_wheel_->rotateLink(quat_d); } catch(ahl_robot::Exception& e) { ROS_ERROR_STREAM(e.what()); } catch(ahl_ctrl::Exception& e) { ROS_ERROR_STREAM(e.what()); } }
void transformF2W(Eigen::Matrix<double,3,4> &m, const Eigen::Matrix<double,4,1> &trans, const Eigen::Quaternion<double> &qrot) { m.block<3,3>(0,0) = qrot.toRotationMatrix(); m.col(3) = trans.head(3); };
Eigen::Matrix< Tp, 4, 4 > transformationMatrix( Eigen::Quaternion< Tp > &quaternion, Eigen::Matrix< Tp, 3, 1 > &translation) { Eigen::Matrix< Tp, 3, 3 > rr = quaternion.toRotationMatrix(); Eigen::Matrix< Tp, 4, 4 > ttmm; ttmm << rr, translation, 0.0, 0.0, 0.0, 1.0; return ttmm; }
/** * @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(); }
void rpyToQuaternion(const Eigen::Vector3d& rpy, Eigen::Quaternion<double>& q) { double a = rpy.coeff(0); double b = rpy.coeff(1); double g = rpy.coeff(2); double sin_b_half = sin(0.5 * b); double cos_b_half = cos(0.5 * b); double diff_a_g_half = 0.5 * (a - g); double sum_a_g_half = 0.5 * (a + g); q.x() = sin_b_half * cos(diff_a_g_half); q.y() = sin_b_half * sin(diff_a_g_half); q.z() = cos_b_half * sin(sum_a_g_half); q.w() = cos_b_half * cos(sum_a_g_half); }
// transforms void transformW2F(Eigen::Matrix<double,3,4> &m, const Eigen::Matrix<double,4,1> &trans, const Eigen::Quaternion<double> &qrot) { m.block<3,3>(0,0) = qrot.toRotationMatrix();//.transpose(); m.col(3).setZero(); // make sure there's no translation m.col(3) = -m*trans; };
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; }
base::Vector6d AuvMotion::gravity_buoyancy(const Eigen::Quaternion<double> q) { base::Vector6d gravitybuoyancy; double mass; control->nodes->getNodeMass(vehicle_id ,&mass); double buoyancy = _buoyancy_force; base::Vector3d pos = control->nodes->getPosition(vehicle_id); // In Quaternion form float e1 = q.x(); float e2 = q.y(); float e3 = q.z(); float e4 = q.w(); float xg = 0.0; float yg = 0.0; float zg = 0.0; float xb = centerOfBuoyancy[0]; float yb = centerOfBuoyancy[1]; float zb = centerOfBuoyancy[2];; gravitybuoyancy(0) = 0.0; gravitybuoyancy(1) = 0.0; if(pos[2] + centerOfBuoyancy[2] < 0.0){ //The vehicle is completly under water gravitybuoyancy(2) = buoyancy - mass; }else if(pos[2] < 0.0 && centerOfBuoyancy[2] != 0){ //The vehicle is partwise underwater -> apply buoyancy partwise gravitybuoyancy(2) = (buoyancy * (-pos[2]/ centerOfBuoyancy[2] ) ) - mass; }else{ //The vehicle is over the surface -> no buoyancy gravitybuoyancy(2) = -mass; } //Angular buoancy forces gravitybuoyancy(3) = ((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((yg*mass)-(yb*buoyancy)))+(2*((e4*e1)+(e2*e3))*((zg*mass)-(zb*buoyancy))); gravitybuoyancy(4) =-((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((xg*mass)-(xb*buoyancy)))+(2*((e4*e2)-(e1*e3))*((zg*mass)-(zb*buoyancy))); gravitybuoyancy(5) =-(2*((e4*e1)+(e2*e3))*((xg*mass)-(xb*buoyancy)))-(2*((e4*e2)-(e1*e3))*((yg*mass)-(yb*buoyancy))); //Convert angular forces to world frame gravitybuoyancy.block<3,1>(3,0) = q * gravitybuoyancy.block<3,1>(3,0); return gravitybuoyancy; }
/* * This small example show the conversion between Eigen types and KDL types * * The toKDLxxx() function specify the KDL type in the name because it is impossible * to determine the KDL type at compile time when using a dynamic eigen matrix. */ int main(int argc, char* argv[]) { using namespace KDL; using namespace std; cout << "Demonstration of simple conversion routines between KDL and Eigen types \n"; Vector a(1.0,2.0,3.0); cout << "KDL Vector " << a << "\n"; Eigen::Vector3d a_eigen; a_eigen = toEigen(a); cout << "Eigen representation " << a_eigen.transpose() << "\n"; cout << "converted back to KDL " << toKDLVector(a_eigen) << "\n"; cout << endl; Twist t(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0)); cout << "KDL Vector " << t << "\n"; Eigen::Matrix<double,6,1> t_eigen; t_eigen = toEigen(t); cout << "Eigen representation " << t_eigen.transpose() << "\n"; cout << "converted back to KDL " << toKDLTwist(t_eigen) << "\n"; cout << endl; Wrench w(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0)); cout << "KDL Wrench " << t << "\n"; Eigen::Matrix<double,6,1> w_eigen; w_eigen = toEigen(w); cout << "Eigen representation " << w_eigen.transpose() << "\n"; cout << "converted back to KDL " << toKDLTwist(w_eigen) << "\n"; Rotation R( Rotation::EulerZYX(30*deg2rad,45*deg2rad,0*deg2rad) ); cout << "KDL Rotation " << R << "\n"; Eigen::Matrix<double,3,3> R_eigen; R_eigen = toEigen( R ); cout << "Eigen 3x3 matrix " << R_eigen << "\n"; cout << "converted back to KDL " << toKDLRotation( R_eigen ) << "\n"; Eigen::Quaternion<double> q; q = toEigenQuaternion( R ); cout << "Eigen Quaternion " << q.coeffs() << "\n"; cout << "converted back to KDL " << toKDLRotation( q ) << "\n"; cout << endl; return 0; }
void publishNode(unsigned int index, Eigen::Matrix<double,4,1> trans, Eigen::Quaternion<double> fq, const frame_common::CamParams &cp, bool isFixed, sba::CameraNode &msg) { msg.index = index; msg.transform.translation.x = trans.x(); msg.transform.translation.y = trans.y(); msg.transform.translation.z = trans.z(); msg.transform.rotation.x = fq.x(); msg.transform.rotation.y = fq.y(); msg.transform.rotation.z = fq.z(); msg.transform.rotation.w = fq.w(); msg.fx = cp.fx; msg.fy = cp.fy; msg.cx = cp.cx; msg.cy = cp.cy; msg.baseline = cp.tx; msg.fixed = isFixed; }
int main(int argc, char* argv[]) { ros::init(argc,argv,"navsts2odom"); ros::NodeHandle nh, ph("~"); //Get rotation between the two std::vector<double> rpy(3,0); ph.param("rpy",rpy,rpy); //Setup the LTP to Odom frame Eigen::Quaternion<double> q; labust::tools::quaternionFromEulerZYX(rpy[0], rpy[1], rpy[2],q); Eigen::Matrix3d rot = q.toRotationMatrix().transpose(); ros::Publisher odom = nh.advertise<nav_msgs::Odometry>("uwsim_hook", 1); ros::Subscriber navsts = nh.subscribe<auv_msgs::NavSts>("navsts",1, boost::bind(&onNavSts, boost::ref(odom), boost::ref(rot), _1)); ros::spin(); return 0; }
bool operator()(T const* const* parameters, T* residuals) const { // Map spline UniformSpline<T> spline(spline_dt_, spline_offset_); for (size_t i=0; i < num_knots_; ++i) { spline.add_knot((T*) ¶meters[i][0]); } Eigen::Matrix<T, 3, 1> landmark(T(5.0), T(1.0), T(3.0)); Eigen::Matrix<T, 3, 1> expected(T(0.0), T(0.0), T(1.0)); // Distance to center Sophus::SE3Group<T> P; Eigen::Matrix<T, 4, 4> dP, d2P; for (size_t i=0; i < eval_times_.size(); ++i) { spline.evaluate(T(eval_times_[i]), P, dP, d2P); Eigen::Matrix<T, 3, 1> X_spline = P.inverse() * landmark; Eigen::Matrix<T, 3, 1> diff = X_spline - expected; residuals[3*i + 0] = diff(0); residuals[3*i + 1] = diff(1); residuals[3*i + 2] = diff(2); } const size_t poff = 3 * eval_times_.size(); // Constant angular difference for (size_t i=1; i < eval_times_.size(); ++i) { SE3Group<T> P0, P1, delta; spline.evaluate(T(eval_times_[i-1]), P0, dP, d2P); spline.evaluate(T(eval_times_[i]), P1, dP, d2P); delta = P0.inverse() * P1; Eigen::Quaternion<T> q = delta.unit_quaternion(); residuals[poff + 4*(i - 1) + 0] = T(5.0) * (q.w() - T(cos(0.5 * expected_angular_difference_))); residuals[poff + 4*(i - 1) + 1] = T(5.0) * (q.x() - T(sin(0.5 * expected_angular_difference_))); residuals[poff + 4*(i - 1) + 2] = T(5.0) * q.y(); //- 0; residuals[poff + 4*(i - 1) + 3] = T(5.0) * q.z(); //- 0; } return true; }
OBAPI void SetTorsion(double *c, unsigned int ref[4], double setang, std::vector<int> atoms) { unsigned int cidx[4]; cidx[0] = (ref[0] - 1) * 3; cidx[1] = (ref[1] - 1) * 3; cidx[2] = (ref[2] - 1) * 3; cidx[3] = (ref[3] - 1) * 3; const Eigen::Vector3d va( c[cidx[0]], c[cidx[0]+1], c[cidx[0]+2] ); const Eigen::Vector3d vb( c[cidx[1]], c[cidx[1]+1], c[cidx[1]+2] ); const Eigen::Vector3d vc( c[cidx[2]], c[cidx[2]+1], c[cidx[2]+2] ); const Eigen::Vector3d vd( c[cidx[3]], c[cidx[3]+1], c[cidx[3]+2] ); // calculate the rotation angle const double ang = setang - DEG_TO_RAD * VectorTorsion(va, vb, vc, vd); // the angles are the same... if (fabs(ang) < 1e-5) return; // setup the rotation matrix const Eigen::Vector3d bc = (vb - vc).normalized(); Eigen::Quaternion<double> m; m = Eigen::AngleAxis<double>(-ang, bc); // apply the rotation int j; for (int i = 0; i < atoms.size(); ++i) { j = (atoms[i] - 1) * 3; Eigen::Vector3d vj( c[j], c[j+1], c[j+2] ); vj -= vb; // translate so b is at origin vj = m.toRotationMatrix() * vj; vj += vb; // translate back c[j] = vj.x(); c[j+1] = vj.y(); c[j+2] = vj.z(); } }
bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf) { assert(q_urdf.size()==7); assert(q_sot.size()==6); // ********* RPY to Quat ********* const double r = q_sot[3]; const double p = q_sot[4]; const double y = q_sot[5]; const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX()); const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY()); const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ()); const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle; q_urdf[0 ]=q_sot[0 ]; //BASE q_urdf[1 ]=q_sot[1 ]; q_urdf[2 ]=q_sot[2 ]; q_urdf[3 ]=quat.x(); q_urdf[4 ]=quat.y(); q_urdf[5 ]=quat.z(); q_urdf[6 ]=quat.w(); return true; }
IGL_INLINE void igl::snap_to_fixed_up( const Eigen::Quaternion<Qtype> & q, Eigen::Quaternion<Qtype> & s) { using namespace Eigen; typedef Eigen::Matrix<Qtype,3,1> Vector3Q; const Vector3Q up = q.matrix() * Vector3Q(0,1,0); Vector3Q proj_up(0,up(1),up(2)); if(proj_up.norm() == 0) { proj_up = Vector3Q(0,1,0); } proj_up.normalize(); Quaternion<Qtype> dq; dq = Quaternion<Qtype>::FromTwoVectors(up,proj_up); s = dq * q; }