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;
}
