matrix matrix::covariance(void)const throw(int) {
//covariance matrix is a square matrix
matrix temp_mat(column,column);
float *mean_ptr;
if(!(isOrderValid(row, column) && temp_mat.isOrderValid(temp_mat.row, temp_mat.column)))
throw INVALID_ORDER;
try{
mean_ptr = new float[column];
}
catch(std::bad_alloc &ba){
throw ALLOCATION_FAILURE;
}
int i,j,k;
//proceed further only if enough memory is available
for(i=0; i<column; i++){
mean_ptr[i]=0;
for(j=0; j<row; j++){
mean_ptr[i] += p_mat[j*column+i];
}
//Store mean for each column
mean_ptr[i] /= row;
}
//Find covariance for ith and jth columns
for(i=0; i<column; i++) {
for(j=0; j<column; j++) {
temp_mat.p_mat[i*column+j] = 0;
for(k=0; k<row; k++){
temp_mat.p_mat[i*column+j] += (p_mat[k*column+i] - mean_ptr[i]) * (p_mat[k*column+j] - mean_ptr[j]);
}
temp_mat.p_mat[i*column+j] /= row;
}
}
return (temp_mat);
}//covariance() ends
//--------------------------------------------------------------------
// setWorldRotation()
//--------------------------------------------------------------------
void LLJoint::setWorldRotation( const LLQuaternion& rot )
{
if (mParent == NULL)
{
this->setRotation( rot );
return;
}
LLMatrix4a parentWorldMatrix = mParent->getWorldMatrix();
LLQuaternion2 rota(rot);
LLMatrix4a temp_mat(rota);
LLMatrix4a invParentWorldMatrix = mParent->getWorldMatrix();
invParentWorldMatrix.setTranslate_affine(LLVector3(0.f));
invParentWorldMatrix.invert();
invParentWorldMatrix.mul(temp_mat);
setRotation(LLQuaternion(LLMatrix4(invParentWorldMatrix.getF32ptr())));
}
//--------------------------------------------------------------------
// setWorldRotation()
//--------------------------------------------------------------------
void LLJoint::setWorldRotation( const LLQuaternion& rot )
{
if (mParent == NULL)
{
this->setRotation( rot );
return;
}
LLMatrix4 temp_mat(rot);
LLMatrix4 parentWorldMatrix = mParent->getWorldMatrix();
parentWorldMatrix.mMatrix[VW][VX] = 0;
parentWorldMatrix.mMatrix[VW][VY] = 0;
parentWorldMatrix.mMatrix[VW][VZ] = 0;
LLMatrix4 invParentWorldMatrix = parentWorldMatrix.invert();
temp_mat *= invParentWorldMatrix;
setRotation(LLQuaternion(temp_mat));
}
/**
* Returns the determinant of the matrix
* throws: ALLOCATION_FAILURE, OPERATION_FAILURE, DETERMINANT_ZERO
*/
float matrix::determinant(void)const throw(int){
//Calculate determinant only if square matrix else throw an exception
if(!isOrderValid(row, column)){
throw OPERATION_FAILURE;
}
//temporary matrix = input matrix
matrix temp_mat(row,row);
float determinant_value = 1;
int i,j,k,m,n,flagger=0;
float ratio=0,temp=0,count=0;
for(i=0;i<row*row;i++)
//the input matrix is used to perform required operations
temp_mat.p_mat[i] = p_mat[i];
for(i=0;i<row;i++)
{
flagger=i;
//Consider only the PD elements
while(temp_mat.p_mat[i*row+i] == 0)
{
if(flagger == (row-1))
return 0;
else
{
flagger++;
for(k=0;k<row;k++)
{
temp = temp_mat.p_mat[k*row+i];
temp_mat.p_mat[k*row+i] = temp_mat.p_mat[k*row+flagger];
temp_mat.p_mat[k*row+flagger] = temp;
}
}//else ends
} // while ends...repeat till (n-1) columns have been swapped ie max limit
for(j=0;j<row;j++)
{
if(j!=i)
//find the ratio by considering only the PD elements.
ratio = temp_mat.p_mat[j*row+i] / temp_mat.p_mat[i*row+i];
else
continue;
for(k=0;k<row;k++)
{
temp_mat.p_mat[j*row+k] -= ratio* temp_mat.p_mat[i*row+k];
}// k ends
for(m=0;m<row;m++)
{
count=0;
for(n=0;n<row;n++)
{
if(temp_mat.p_mat[m*row+n]==0)
count++;
}
if(count==row)
return 0;
}//m loop ends
}//j ends
} //i ends
for(i=0;i<row;i++)
{
determinant_value *= temp_mat.p_mat[i*row+i];
}
return (determinant_value);
}
/**
* Returns the inverse of the matrix. If no inverse exists NO_INVERSE_EXISTS exception is thrown
* throws: NO_INVERSE_EXISTS, ALLOCATION_FAILURE
*/
matrix matrix::inverse(void)const throw(int){
//temporary matrix = input matrix
matrix temp_mat(row,row);
//[I] matrix to calculate the inverse
matrix inv(row,column);
if(!(isOrderValid(row, column) && (row == column)))
throw NO_INVERSE_EXISTS;
int i, j, k, m, n, flagger = 0;
float ratio = 0, temp = 0, count = 0;
//start by considering answer as [I] matrix.
for(i=0;i<row;i++)
inv.p_mat[i*row+i] = 1;
//Copy the input matrix is used to perform required operations
for(i=0;i<row*row;i++)
temp_mat.p_mat[i] = p_mat[i];
for(i=0; i<row; i++){
flagger = i;
//find inverse by Gauss Elimination method.
//Consider only the PD elements
while(temp_mat.p_mat[i*row + i] == 0){
//flagger indicates the Max No. of failed attempts of exchanging the diff columns.
if(flagger == (row - 1)){
throw NO_INVERSE_EXISTS;
}
else{
flagger++;
//swap the columns and check again
for(k=0; k<row; k++){
temp = temp_mat.p_mat[k*row+i];
temp_mat.p_mat[k*row+i] = temp_mat.p_mat[k*row+flagger];
temp_mat.p_mat[k*row+flagger] = temp;
//whatever exchanges done to base matrix do similar changes to answer matrix
temp = inv.p_mat[k*row+i];
inv.p_mat[k*row+i] = inv.p_mat[k*row+flagger];
inv.p_mat[k*row+flagger] = temp;
}
}//else ends
} // while ends...repeat till (n-1) columns have been swapped ie max limit
for(j=0;j<row;j++){
if(j!=i){
//find the ratio by considering only the PD elements
ratio = temp_mat.p_mat[j*row+i] / temp_mat.p_mat[i*row+i];
}
else{
continue;
}
for(k=0; k<row; k++){
temp_mat.p_mat[j*row+k] = temp_mat.p_mat[j*row+k] - ratio * (temp_mat.p_mat[i*row+k]);
inv.p_mat[j*row+k] = inv.p_mat[j*row+k] - ratio*(inv.p_mat[i*row+k]);
}// k ends
for(m=0; m<row; m++){
count=0;
for(n=0;n<row;n++){
//check after every manipulation whether all elements of a row of input matrix are 0.
if(temp_mat.p_mat[m*row+n] == 0)
count++;
}
//If all elements of a row are 0...
if(count == row){
throw NO_INVERSE_EXISTS;
}
}//m loop ends
}//j ends
} //i ends
//Divide each element of a row with corresponding PD element
for(i=0;i<row;i++){
for(j=0;j<row;j++){
inv.p_mat[i*row+j] /= temp_mat.p_mat[i*row+i];
}
}
return (inv);
}
void HintedHandleEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const geometry_msgs::PointStampedConstPtr &point_msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PassThrough<pcl::PointXYZ> pass;
int K = 1;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
geometry_msgs::PointStamped transed_point;
ros::Time now = ros::Time::now();
try
{
listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
}
catch(tf::TransformException ex)
{
JSK_ROS_ERROR("%s", ex.what());
return;
}
pcl::PointXYZ searchPoint;
searchPoint.x = transed_point.point.x;
searchPoint.y = transed_point.point.y;
searchPoint.z = transed_point.point.z;
//remove too far cloud
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
pass.filter(*cloud);
if(cloud->points.size() < 10){
JSK_ROS_INFO("points are too small");
return;
}
if(1){ //estimate_normal
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(kd_tree);
ne.setRadiusSearch(0.02);
ne.setViewPoint(0, 0, 0);
ne.compute(*cloud_normals);
}
else{ //use normal of msg
}
if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
JSK_ROS_INFO("kdtree failed");
return;
}
float x = cloud->points[pointIdxNKNSearch[0]].x;
float y = cloud->points[pointIdxNKNSearch[0]].y;
float z = cloud->points[pointIdxNKNSearch[0]].z;
float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
double theta = acos(v_x);
// use normal for estimating handle direction
tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
tf::Quaternion final_quaternion = normal;
double min_theta_index = 0;
double min_width = 100;
tf::Quaternion min_qua(0, 0, 0, 1);
visualization_msgs::Marker debug_hand_marker;
debug_hand_marker.header = cloud_msg->header;
debug_hand_marker.ns = string("debug_grasp");
debug_hand_marker.id = 0;
debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
debug_hand_marker.pose.orientation.w = 1;
debug_hand_marker.scale.x=0.003;
tf::Matrix3x3 best_mat;
//search 180 degree and calc the shortest direction
for(double theta_=0; theta_<3.14/2;
theta_+=3.14/2/30){
tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
tf::Quaternion temp_qua = normal * rotate_;
tf::Matrix3x3 temp_mat(temp_qua);
geometry_msgs::Pose pose_respected_to_tf;
pose_respected_to_tf.position.x = x;
pose_respected_to_tf.position.y = y;
pose_respected_to_tf.position.z = z;
pose_respected_to_tf.orientation.x = temp_qua.getX();
pose_respected_to_tf.orientation.y = temp_qua.getY();
pose_respected_to_tf.orientation.z = temp_qua.getZ();
pose_respected_to_tf.orientation.w = temp_qua.getW();
Eigen::Affine3d box_pose_respected_to_cloud_eigend;
tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
= box_pose_respected_to_cloud_eigend.inverse();
Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
= box_pose_respected_to_cloud_eigend_inversed.matrix();
jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
box_pose_respected_to_cloud_eigen_inversed_matrixd,
box_pose_respected_to_cloud_eigen_inversed_matrixf);
Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output_cloud, offset);
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(output_cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
pass.filter(*points_z);
pass.setInputCloud(points_z);
pass.setFilterFieldName("z");
pass.setFilterLimits(-handle.finger_d, handle.finger_d);
pass.filter(*points_yz);
pass.setInputCloud(points_yz);
pass.setFilterFieldName("x");
pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
pass.filter(*points_xyz);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
for(size_t index=0; index<points_xyz->size(); index++){
points_xyz->points[index].x = points_xyz->points[index].z = 0;
}
if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
kdtree.setInputCloud(points_xyz);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZ search_point_tree;
search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
double before_w=10, temp_w;
for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
temp_w =sqrt(pointRadiusSquaredDistance[index]);
if(temp_w - before_w > handle.finger_w*2){
break; // there are small space for finger
}
before_w=temp_w;
}
if(before_w < min_width){
min_theta_index = theta_;
min_width = before_w;
min_qua = temp_qua;
best_mat = temp_mat;
}
//for debug view
geometry_msgs::Point temp_point;
std_msgs::ColorRGBA temp_color;
temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
}
}
geometry_msgs::PoseStamped handle_pose_stamped;
handle_pose_stamped.header = cloud_msg->header;
handle_pose_stamped.pose.position.x = x;
handle_pose_stamped.pose.position.y = y;
handle_pose_stamped.pose.position.z = z;
handle_pose_stamped.pose.orientation.x = min_qua.getX();
handle_pose_stamped.pose.orientation.y = min_qua.getY();
handle_pose_stamped.pose.orientation.z = min_qua.getZ();
handle_pose_stamped.pose.orientation.w = min_qua.getW();
std_msgs::Float64 min_width_msg;
min_width_msg.data = min_width;
pub_pose_.publish(handle_pose_stamped);
pub_debug_marker_.publish(debug_hand_marker);
pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
jsk_recognition_msgs::SimpleHandle simple_handle;
simple_handle.header = handle_pose_stamped.header;
simple_handle.pose = handle_pose_stamped.pose;
simple_handle.handle_width = min_width;
pub_handle_.publish(simple_handle);
}
