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;
}
// 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;
}
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();
}
}
}
/**
* @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 ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
{
// node references
Node &nr = nodes[ndr];
Matrix<double,4,1> &tr = nr.trans;
Quaternion<double> &qr = nr.qrot;
Node &n1 = nodes[nd1];
Matrix<double,4,1> &t1 = n1.trans;
Quaternion<double> &q1 = n1.qrot;
// first get the second frame in first frame coords
Eigen::Matrix<double,3,1> pc = nr.w2n * t1;
// Jacobians wrt first frame parameters
// translational part of 0p1 wrt translational vars of p0
// this is just -R0' [from 0t1 = R0'(t1 - t0)]
J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
// translational part of 0p1 wrt rotational vars of p0
// dR'/dq * [pw - t]
Eigen::Matrix<double,3,1> pwt;
pwt = (t1-tr).head(3); // transform translations
// dx
Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
J0.block<3,1>(0,3) = dp;
// dy
dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
J0.block<3,1>(0,4) = dp;
// dz
dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
J0.block<3,1>(0,5) = dp;
// rotational part of 0p1 wrt translation vars of p0 => zero
J0.block<3,3>(3,0).setZero();
// rotational part of 0p1 wrt rotational vars of p0
// from 0q1 = qpmean * s0' * q0' * q1
// dqdx
Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
qr1.coeffs() = q1.coeffs();
qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x()); // qpmean * ds0'/dx
qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
qr0 = qr0*qr1; // rotate to zero mean
qrd = qpmean*qr0; // for normalization check
qrn = qrn*qr0;
#ifdef NORMALIZE_Q
if (qrd.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J0.block<3,1>(3,3) = qrn.vec();
// dqdy
qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y()); // qpmean * ds0'/dy
qrn = qrn*qr0;
#ifdef NORMALIZE_Q
if (qrd.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J0.block<3,1>(3,4) = qrn.vec();
// dqdz
qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z()); // qpmean * ds0'/dz
qrn = qrn*qr0;
#ifdef NORMALIZE_Q
if (qrd.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J0.block<3,1>(3,5) = qrn.vec();
// transpose
J0t = J0.transpose();
// cout << endl << "J0 " << ndr << endl << J0 << endl;
// Jacobians wrt second frame parameters
// translational part of 0p1 wrt translational vars of p1
// this is just R0' [from 0t1 = R0'(t1 - t0)]
J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
// translational part of 0p1 wrt rotational vars of p1: zero
J1.block<3,3>(0,3).setZero();
// rotational part of 0p1 wrt translation vars of p0 => zero
J1.block<3,3>(3,0).setZero();
// rotational part of 0p1 wrt rotational vars of p0
// from 0q1 = q0'*s1*q1
Eigen::Quaternion<double> qrc;
qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
qrc = qpmean*qrc*qr1; // mean' * qr0' * qr1
qrc.normalize();
// cout << endl << "QRC : " << qrc.coeffs().transpose() << endl;
double dq = 1.0e-8;
double wdq = 1.0 - dq*dq;
qr1.coeffs() = Vector4d(dq,0,0,wdq);
// cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;
// cout << "QRdx: " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;
// dqdx
qrn.coeffs() = Vector4d(1,0,0,0);
qrn = qrc*qrn;
// cout << "J1dx: " << qrn.coeffs().transpose() << endl;
#ifdef NORMALIZE_Q
if (qrc.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J1.block<3,1>(3,3) = qrn.vec();
// dqdy
qrn.coeffs() = Vector4d(0,1,0,0);
qrn = qrc*qrn;
#ifdef NORMALIZE_Q
if (qrc.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J1.block<3,1>(3,4) = qrn.vec();
// dqdz
qrn.coeffs() = Vector4d(0,0,1,0);
qrn = qrc*qrn;
#ifdef NORMALIZE_Q
if (qrc.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J1.block<3,1>(3,5) = qrn.vec();
// transpose
J1t = J1.transpose();
// cout << endl << "J1 " << nd1 << endl << J1 << endl;
};
void get3DMoments(vector<Point> feature_points, int feat_index, int index)
{
// for(int i=0; i<feature_points.size(); i++)
// ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
//ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
//Extract the indices for the points in the point cloud data
pcl::PointIndices point_indices;
for(int i=0; i<feature_points.size(); i++)
{
//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
}
//ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
Eigen::Vector4f centroid;
Eigen::Matrix3f covariance_matrix;
// Estimate the XYZ centroid
pcl::compute3DCentroid (pcl_in, point_indices, centroid);
#ifdef DEBUG
ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif
//ROS_INFO("Computing Covariance ");
//Compute the centroid and the covariance of the points
computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
//Print the 3D Moments
//ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif
for(int i=0; i<3; i++)
{
feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
for(int j=0; j<3; j++)
{
feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
}
}
//Get the principal components and the quaternion
Eigen::Matrix3f evecs;
Eigen::Vector3f evals;
pcl::eigen33 (covariance_matrix, evecs, evals);
Eigen::Matrix3f rotation;
rotation.row (0) = evecs.col (0);
rotation.row (1) = evecs.col (1);
//rotation.row (2) = evecs.col (2);
rotation.row (2) = rotation.row (0).cross (rotation.row (1));
//rotation.transposeInPlace ();
#ifdef DEBUG
std::cerr << "Rotation matrix: " << endl;
std::cerr << rotation << endl;
std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif
rotation.transposeInPlace ();
Eigen::Quaternion<float> qt (rotation);
qt.normalize ();
//Publish Marker
visualization_msgs::Marker marker;
marker.header.frame_id = "/openni_rgb_optical_frame";
marker.header.stamp = ros::Time().now();
marker.ns = "Triangulation";
marker.id = index+1;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(5);
//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();
marker.scale.x = sqrt(evals(0)) *2;
marker.scale.y = sqrt(evals(1)) *2;
marker.scale.z = sqrt(evals(2)) *2;
//red
marker.color.a = 0.5;
marker.color.r = rand()/((double)RAND_MAX + 1);
marker.color.g = rand()/((double)RAND_MAX + 1);
marker.color.b = rand()/((double)RAND_MAX + 1);
object_pose_marker_pub_.publish(marker);
}
