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);
}
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;
}
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);
};
// 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;
};
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;
}
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();
}
}
