/******************************************************************************
* Rectangle estimation service.
*
* @param req Request of type EstimateRectAlt.
* @param res Response of type EstimateRectAlt.
*/
bool estimateRectAlt_callback(srs_env_model_percp::EstimateRectAlt::Request &req,
srs_env_model_percp::EstimateRectAlt::Response &res
)
{
// Vertices (corners) defining the bounding box.
// Each is noted using this template:
// bb{L=left,R=right}{B=bottom,T=top}{F=front,B=back}
// (This notation provides more readable code, in comparison with
// e.g. an array with 8 items.)
Point3f bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB;
// Vector with pointers to the BB vertices (to be able to iterate over them)
vector<Point3f *> bbVertices(8);
bbVertices[0] = &bbLBF;
bbVertices[1] = &bbRBF;
bbVertices[2] = &bbRTF;
bbVertices[3] = &bbLTF;
bbVertices[4] = &bbLBB;
bbVertices[5] = &bbRBB;
bbVertices[6] = &bbRTB;
bbVertices[7] = &bbLTB;
// Calculate coordinates of corners of the bounding box relative to its center
bbLBF.x = -req.bounding_box_lwh.x;
bbLBF.y = -req.bounding_box_lwh.y;
bbLBF.z = 0.0f;
bbLTF.x = -req.bounding_box_lwh.x;
bbLTF.y = -req.bounding_box_lwh.y;
bbLTF.z = req.bounding_box_lwh.z;
bbRBF.x = req.bounding_box_lwh.x;
bbRBF.y = -req.bounding_box_lwh.y;
bbRBF.z = 0.0f;
bbRTF.x = req.bounding_box_lwh.x;
bbRTF.y = -req.bounding_box_lwh.y;
bbRTF.z = req.bounding_box_lwh.z;
bbRBB.x = req.bounding_box_lwh.x;
bbRBB.y = req.bounding_box_lwh.y;
bbRBF.z = 0.0f;
bbRTB.x = req.bounding_box_lwh.x;
bbRTB.y = req.bounding_box_lwh.y;
bbRTB.z = req.bounding_box_lwh.z;
bbLBB.x = -req.bounding_box_lwh.x;
bbLBB.y = req.bounding_box_lwh.y;
bbRBF.z = 0.0f;
bbLTB.x = -req.bounding_box_lwh.x;
bbLTB.y = req.bounding_box_lwh.y;
bbLTB.z = req.bounding_box_lwh.z;
// Apply the rotation and translation stored in the Pose parameter
tf::Quaternion q(req.pose.orientation.x,
req.pose.orientation.y,
req.pose.orientation.z,
req.pose.orientation.w);
btTransform trMat(q);
for( int i = 0; i < (int)bbVertices.size(); i++ )
{
btVector3 res = trMat * btVector3(bbVertices[i]->x, bbVertices[i]->y, bbVertices[i]->z);
bbVertices[i]->x = res.x() + req.pose.position.x;
bbVertices[i]->y = res.y() + req.pose.position.y;
bbVertices[i]->z = res.z() + req.pose.position.z;
}
// Estimate the rectangle
point2_t p1, p2;
if( !estimateRect(req.header.stamp,
bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB,
p1, p2) )
{
return false;
}
// Set response
//--------------------------------------------------------------------------
res.p1[0] = p1[0];
res.p1[1] = p1[1];
res.p2[0] = p2[0];
res.p2[1] = p2[1];
// Log request timestamp
//--------------------------------------------------------------------------
ROS_INFO("Request timestamp: %d.%d", req.header.stamp.sec, req.header.stamp.nsec);
return true;
}
Vector<double> fromCamera2World(Vector<double> posInCamera, Camera cam)
{
Matrix<double> rotMat = cam.get_R();
Vector<double> posCam = cam.get_t();
Matrix<double> trMat(4,4,0.0);
trMat(3,3) = 1;
trMat(0,0) = rotMat(0,0);
trMat(0,1) = rotMat(0,1);
trMat(0,2) = rotMat(0,2);
trMat(1,0) = rotMat(1,0);
trMat(1,1) = rotMat(1,1);
trMat(1,2) = rotMat(1,2);
trMat(2,0) = rotMat(2,0);
trMat(2,1) = rotMat(2,1);
trMat(2,2) = rotMat(2,2);
posCam *= Globals::WORLD_SCALE;
trMat(3,0) = posCam(0);
trMat(3,1) = posCam(1);
trMat(3,2) = posCam(2);
Vector<double> transpoint = trMat*posInCamera;
return transpoint;
}
