/**
* @brief estimated_pose Returns estimated object pose in cartesian frame
*/
geometry_msgs::Pose kalman_pose_estimation(geometry_msgs::Pose previous_pose_, geometry_msgs::Pose measured_pose_) {
// TODO: create kalman filter for each estimated object separatelly.
cv::KalmanFilter KF; // instantiate Kalman Filter
int nStates = 18; // the number of states
int nMeasurements = 6; // the number of measured states
int nInputs = 0; // the number of action control
double dt = 0.125; // time between measurements (1/FPS)
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function
cv::Mat measurements(nMeasurements, 1, CV_64F);
measurements.setTo(cv::Scalar(0));
// Get object pose in sensor frame.
tf::Transform measured_tf;
tf::poseMsgToTF (measured_pose_ , measured_tf);
// tf::btMatrix3x3 measured_rot = measured_tf.getBasis();
// Get the measured translation
cv::Mat translation_measured(3, 1, CV_64F);
// translation_measured = ... TODO!
// Get the measured rotation
cv::Mat rotation_measured(3, 3, CV_64F);
//rotation_measured = ... TODO!
// fill the measurements vector
fillMeasurements(measurements, translation_measured, rotation_measured);
// Instantiate estimated translation and rotation
cv::Mat translation_estimated(3, 1, CV_64F);
cv::Mat rotation_estimated(3, 3, CV_64F);
// update the Kalman filter with good measurements
updateKalmanFilter( KF, measurements, translation_estimated, rotation_estimated);
geometry_msgs::Pose estimated_pose;
return estimated_pose;
}
void DoorHandleDetectionNode::mainComputation(const sensor_msgs::PointCloud2::ConstPtr &image)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_bbox(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sandwich(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_debug(new pcl::PointCloud<pcl::PointXYZ>);
vpColVector direction_line(3);
vpColVector normal(3);
vpHomogeneousMatrix dMp;
geometry_msgs::PoseStamped cMdh_msg;
vpColVector coeffs;
struct inliersAndCoefficients plane_variables;
pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
std_msgs::Int8 status_handle_msg;
//Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*image, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//Downsample the point cloud given by the camera
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud( cloud );
vg.setLeafSize( 0.005f, 0.005f, 0.005f );
vg.filter(*cloud_filtered);
if(m_cam_is_initialized)
{
//Find the coefficients of the biggest plane of the point cloud
plane_variables = DoorHandleDetectionNode::getPlaneInliersAndCoefficients(cloud_filtered);
coeffs.stack(plane_variables.coefficients->values[0]);
coeffs.stack(plane_variables.coefficients->values[1]);
coeffs.stack(plane_variables.coefficients->values[2]);
coeffs.stack(plane_variables.coefficients->values[3]);
//Invert the normal in order to have the z axis toward the camera
normal[0] = -coeffs[0];
normal[1] = -coeffs[1];
normal[2] = -coeffs[2];
//Check if the size of the plane if big enough
if (plane_variables.inliers->indices.size() < 100)
{
std::cout << "Could not find a plane in the scene." << std::endl;
vpDisplay::displayText(m_img_, 60, 5, "No plane detected", vpColor::red);
vpDisplay::flush(m_img_);
}
else
{
if (debug)
{
//Copy the inliers of the plane to a new cloud.
plane = DoorHandleDetectionNode::createPlanePC(cloud, plane_variables.inliers, plane_variables.coefficients);
//Create the center of the plane
vpColVector centroidPlane(3);
centroidPlane = DoorHandleDetectionNode::getCentroidPCL(plane);
//Create a tf for the plane
dMp = DoorHandleDetectionNode::createTFPlane(coeffs, centroidPlane[0], centroidPlane[1], centroidPlane[2]);
}
//Compute the Z of the corners of the smaller detection
m_Z_topleft = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_min + coeffs[1]*m_y_min + coeffs[2]) ;
m_Z_topright = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_max + coeffs[1]*m_y_min + coeffs[2]) ;
m_Z_bottomright = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_max + coeffs[1]*m_y_max + coeffs[2]) ;
m_Z_bottomleft = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_min + coeffs[1]*m_y_max + coeffs[2]) ;
//Creating a cloud with all the points of the door handle
if (m_tracking_works)
{
cloud_bbox = DoorHandleDetectionNode::getOnlyUsefulHandle(cloud);
cloud_sandwich = DoorHandleDetectionNode::createPCLSandwich(cloud_bbox, coeffs);
}
else
cloud_sandwich = DoorHandleDetectionNode::createPCLSandwich(cloud, coeffs);
if (debug)
{
//Create a point cloud of only the top left and bottom right points used for the narrow detection
cloud_debug->width = 2;
cloud_debug->height = 1;
cloud_debug->points.resize (cloud_debug->width * cloud_debug->height);
cloud_debug->points[0].x = m_X_min;
cloud_debug->points[0].y = m_Y_min;
cloud_debug->points[0].z = m_Z_topleft;
cloud_debug->points[1].x = m_X_max;
cloud_debug->points[1].y = m_Y_max;
cloud_debug->points[1].z = m_Z_bottomright;
cloud_debug->header.frame_id = m_parent_depth_tf;
debug_pcl_pub.publish(cloud_debug);
//Publish the point cloud of the door handle with noise
cloud_sandwich->header.frame_id = m_parent_depth_tf;
pcl_dh_pub.publish(*cloud_sandwich);
//Publish the point clouds of the plane
plane->header.frame_id = m_parent_depth_tf;
pcl_plane_pub.publish(*plane);
}
//Check if the size of the point cloud of the door handle if big enough to localize it or not and check if the detection should be done or not
if (cloud_sandwich->size()<50 || m_stop_detection){
m_is_door_handle_present = 0;
ROS_INFO_STREAM("No door handle detected : " << cloud_sandwich->size());
m_tracking_works = false;
vpPixelMeterConversion::convertPoint(m_cam_depth, 0, 0, m_x_min, m_y_min);
vpPixelMeterConversion::convertPoint(m_cam_depth, m_img_mono.getWidth() - 1, m_img_mono.getHeight() - 1, m_x_max, m_y_max);
}
else
{
m_is_door_handle_present = 1;
//Get the axis from the cloud sandwich with the odr method
direction_line = getCoeffLineWithODR(cloud_sandwich);
//Set the axis of the door handle towards the right way
if (m_dh_right)
{
if (direction_line[0]<0)
{
direction_line[0] = -direction_line[0];
direction_line[1] = -direction_line[1];
direction_line[2] = -direction_line[2];
}
}
else
{
if (direction_line[0]>0)
{
direction_line[0] = -direction_line[0];
direction_line[1] = -direction_line[1];
direction_line[2] = -direction_line[2];
}
}
//Create the black&white image used to reduce the detection
DoorHandleDetectionNode::morphoSandwich(cloud_sandwich );
//Get the centroid of the door handle
vpColVector centroidDH = DoorHandleDetectionNode::getCentroidPCL(cloud_sandwich);
//Compute the pose of the door handle with respect to the Depth camera and publish a tf of it
m_dMh = DoorHandleDetectionNode::createTFLine(direction_line, normal, centroidDH[0], centroidDH[1], centroidDH[2]);
vpHomogeneousMatrix cMd;
for(unsigned int i=0; i<3; i++)
cMd[i][3] = m_extrinsicParam[i];
//Compute the pose of the door handle with respect to the RGB camera
m_cMh = cMd * m_dMh;
//Kalman Filter
vpTranslationVector T_cMh = m_cMh.getTranslationVector();
vpRotationMatrix R_cMh = m_cMh.getRotationMatrix();
cv::Mat translation_measured(3, 1, CV_64F);
cv::Mat rotation_measured(3, 3, CV_64F);
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
rotation_measured.at<double>(i,j) = R_cMh[i][j];
}
translation_measured.at<double>(i) = T_cMh[i];
}
cv::Mat measurements(6, 1, CV_64F);
// fill the measurements vector
fillMeasurements(measurements, translation_measured, rotation_measured);
// Instantiate estimated translation and rotation
cv::Mat translation_estimated(3, 1, CV_64F);
cv::Mat rotation_estimated(3, 3, CV_64F);
// update the Kalman filter with good measurements
updateKalmanFilter( m_KF, measurements, translation_estimated, rotation_estimated);
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
m_cMh_filtered_kalman[i][j] = rotation_estimated.at<double>(i,j) ;
}
m_cMh_filtered_kalman[i][3] = translation_estimated.at<double>(i);
}
//Publish the pose of the handle with respect to the camera
cMdh_msg.header.stamp = ros::Time::now();
cMdh_msg.pose = visp_bridge::toGeometryMsgsPose(m_cMh_filtered_kalman);
cMdh_msg.header.frame_id = m_parent_rgb_tf;
pose_handle_pub.publish(cMdh_msg);
}
}
}
//Publish the status of the door handle : 0 if no door handle, 1 if a door handle is found
status_handle_msg.data = m_is_door_handle_present;
door_handle_status_pub.publish( status_handle_msg );
}