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;
}
bool Utils::
factorViewMatrix(const Eigen::Projective3f& iMatrix,
Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose,
bool& oIsOrthographic) {
oIsOrthographic = isOrthographic(iMatrix.matrix());
// get appropriate rows
std::vector<int> rows = {0,1,2};
if (!oIsOrthographic) rows[2] = 3;
// get A matrix (upper left 3x3) and t vector
Eigen::Matrix3f A;
Eigen::Vector3f t;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
A(i,j) = iMatrix(rows[i],j);
}
t[i] = iMatrix(rows[i],3);
}
// determine translation vector
oPose.setIdentity();
oPose.translation() = -(A.inverse()*t);
// determine calibration matrix
Eigen::Matrix3f AAtrans = A*A.transpose();
AAtrans.col(0).swap(AAtrans.col(2));
AAtrans.row(0).swap(AAtrans.row(2));
Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans);
oCalib = llt.matrixU();
oCalib.col(0).swap(oCalib.col(2));
oCalib.row(0).swap(oCalib.row(2));
oCalib.transposeInPlace();
// compute rotation matrix
oPose.linear() = (oCalib.inverse()*A).transpose();
return true;
}
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);
}
