template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::track (const PointCloudInConstPtr& prev_input,
const PointCloudInConstPtr& input,
const std::vector<FloatImageConstPtr>& prev_pyramid,
const std::vector<FloatImageConstPtr>& pyramid,
const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
pcl::PointCloud<pcl::PointUV>::Ptr& keypoints,
std::vector<int>& status,
Eigen::Affine3f& motion) const
{
std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->size ());
Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f);
pcl::TransformationFromCorrespondences transformation_computer;
const int nb_points = prev_keypoints->size ();
for (int level = nb_levels_ - 1; level >= 0; --level)
{
const FloatImage& prev = *(prev_pyramid[level*3]);
const FloatImage& next = *(pyramid[level*3]);
const FloatImage& grad_x = *(prev_pyramid[level*3+1]);
const FloatImage& grad_y = *(prev_pyramid[level*3+2]);
Eigen::ArrayXXf prev_win (track_height_, track_width_);
Eigen::ArrayXXf grad_x_win (track_height_, track_width_);
Eigen::ArrayXXf grad_y_win (track_height_, track_width_);
float ratio (1./(1 << level));
for (int ptidx = 0; ptidx < nb_points; ptidx++)
{
Eigen::Array2f prev_pt (prev_keypoints->points[ptidx].u * ratio,
prev_keypoints->points[ptidx].v * ratio);
Eigen::Array2f next_pt;
if (level == nb_levels_ -1)
next_pt = prev_pt;
else
next_pt = next_pts[ptidx]*2.f;
next_pts[ptidx] = next_pt;
Eigen::Array2i iprev_point;
prev_pt -= half_win;
iprev_point[0] = floor (prev_pt[0]);
iprev_point[1] = floor (prev_pt[1]);
if (iprev_point[0] < -track_width_ || (uint32_t) iprev_point[0] >= grad_x.width ||
iprev_point[1] < -track_height_ || (uint32_t) iprev_point[1] >= grad_y.height)
{
if (level == 0)
status [ptidx] = -1;
continue;
}
float a = prev_pt[0] - iprev_point[0];
float b = prev_pt[1] - iprev_point[1];
Eigen::Array4f weight;
weight[0] = (1.f - a)*(1.f - b);
weight[1] = a*(1.f - b);
weight[2] = (1.f - a)*b;
weight[3] = 1 - weight[0] - weight[1] - weight[2];
Eigen::Array3f covar = Eigen::Array3f::Zero ();
spatialGradient (prev, grad_x, grad_y, iprev_point, weight, prev_win, grad_x_win, grad_y_win, covar);
float det = covar[0]*covar[2] - covar[1]*covar[1];
float min_eigenvalue = (covar[2] + covar[0] - std::sqrt ((covar[0]-covar[2])*(covar[0]-covar[2]) + 4.f*covar[1]*covar[1]))/2.f;
if (min_eigenvalue < min_eigenvalue_threshold_ || det < std::numeric_limits<float>::epsilon ())
{
status[ptidx] = -2;
continue;
}
det = 1.f/det;
next_pt -= half_win;
Eigen::Array2f prev_delta (0, 0);
for (unsigned int j = 0; j < max_iterations_; j++)
{
Eigen::Array2i inext_pt = next_pt.floor ().cast<int> ();
if (inext_pt[0] < -track_width_ || (uint32_t) inext_pt[0] >= next.width ||
inext_pt[1] < -track_height_ || (uint32_t) inext_pt[1] >= next.height)
{
if (level == 0)
status[ptidx] = -1;
break;
}
a = next_pt[0] - inext_pt[0];
b = next_pt[1] - inext_pt[1];
weight[0] = (1.f - a)*(1.f - b);
weight[1] = a*(1.f - b);
weight[2] = (1.f - a)*b;
weight[3] = 1 - weight[0] - weight[1] - weight[2];
// compute mismatch vector
Eigen::Array2f beta = Eigen::Array2f::Zero ();
mismatchVector (prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
// optical flow resolution
Eigen::Vector2f delta ((covar[1]*beta[1] - covar[2]*beta[0])*det, (covar[1]*beta[0] - covar[0]*beta[1])*det);
// update position
next_pt[0] += delta[0]; next_pt[1] += delta[1];
next_pts[ptidx] = next_pt + half_win;
if (delta.squaredNorm () <= epsilon_)
break;
if (j > 0 && std::abs (delta[0] + prev_delta[0]) < 0.01 &&
std::abs (delta[1] + prev_delta[1]) < 0.01 )
{
next_pts[ptidx][0] -= delta[0]*0.5f;
next_pts[ptidx][1] -= delta[1]*0.5f;
break;
}
// update delta
prev_delta = delta;
}
// update tracked points
if (level == 0 && !status[ptidx])
{
Eigen::Array2f next_point = next_pts[ptidx] - half_win;
Eigen::Array2i inext_point;
inext_point[0] = floor (next_point[0]);
inext_point[1] = floor (next_point[1]);
if (inext_point[0] < -track_width_ || (uint32_t) inext_point[0] >= next.width ||
inext_point[1] < -track_height_ || (uint32_t) inext_point[1] >= next.height)
{
status[ptidx] = -1;
continue;
}
// insert valid keypoint
pcl::PointUV n;
n.u = next_pts[ptidx][0];
n.v = next_pts[ptidx][1];
keypoints->push_back (n);
// add points pair to compute transformation
inext_point[0] = floor (next_pts[ptidx][0]);
inext_point[1] = floor (next_pts[ptidx][1]);
iprev_point[0] = floor (prev_keypoints->points[ptidx].u);
iprev_point[1] = floor (prev_keypoints->points[ptidx].v);
const PointInT& prev_pt = prev_input->points[iprev_point[1]*prev_input->width + iprev_point[0]];
const PointInT& next_pt = input->points[inext_point[1]*input->width + inext_point[0]];
transformation_computer.add (prev_pt.getVector3fMap (), next_pt.getVector3fMap (), 1.0);
}
}
}
motion = transformation_computer.getTransformation ();
}
void QNode::clingoInterfaceHandler(
const segbot_logical_translator_gui::ClingoInterfaceGoalConstPtr &req) {
segbot_logical_translator_gui::ClingoInterfaceResult resp;
if (req->command.op == "approach" || req->command.op == "gothrough") {
std::string door_name = req->command.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
if (door_idx == (size_t)-1) {
// Interface failure
resp.success = false;
resp.status = "Could not resolve argument: " + door_name;
} else {
bwi_mapper::Point2f approach_pt;
float approach_yaw = 0;
bool door_approachable = false;
if (req->command.op == "approach") {
door_approachable = handler_->getApproachPoint(door_idx,
bwi_mapper::Point2f(robot_x_, robot_y_), approach_pt,
approach_yaw);
} else {
door_approachable = handler_->getThroughDoorPoint(door_idx,
bwi_mapper::Point2f(robot_x_, robot_y_), approach_pt,
approach_yaw);
}
if (door_approachable) {
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = false;
display_text_ = req->command.op + " " + door_name;
Q_EMIT updateFrameInfo();
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = "/map"; //TODO
pose.pose.position.x = approach_pt.x;
pose.pose.position.y = approach_pt.y;
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(approach_yaw),
pose.pose.orientation);
float start_time = ros::Time::now().toSec();
bool success = executeRobotGoal(pose);
float cost = ros::Time::now().toSec() - start_time;
resp.success = success;
// Publish the observable fluents
senseDoorProximity(resp.observable_fluents, door_idx);
if (success) {
if (prev_door_idx_ != -1 &&
prev_door_idx_ != door_idx) {
size_t location_idx =
handler_->getLocationIdx(bwi_mapper::Point2f(robot_x_, robot_y_));
std::string at_str = handler_->getLocationString(location_idx);
ce_->addSample(at_str, prev_door_idx_, door_idx, cost);
}
prev_door_idx_ = door_idx;
} else {
prev_door_idx_ = -1;
}
// Close a door is opened previously
if (success) {
if (sim_auto_door_ && close_door_idx_ != -1) {
gh_->closeDoor(close_door_idx_);
close_door_idx_ = -1;
}
}
// We can't sense the door once we go through as we won't be facing
// it. We should only sense a door when we approach it.
if (req->command.op == "approach") {
segbot_logical_translator_gui::ClingoFluent door_open;
door_open.args.push_back(door_name);
if (gh_->isDoorOpen(door_idx)) {
door_open.op = "open";
resp.observable_fluents.push_back(door_open);
} else {
door_open.op = "-open";
resp.observable_fluents.push_back(door_open);
}
}
} else {
// Planning failure
resp.success = false;
resp.status = "Cannot approach " + door_name + " from here.";
}
}
} else if (req->command.op == "opendoor") {
std::string door_name = req->command.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
if (door_idx == (size_t)-1) {
resp.success = false;
resp.status = "Could not resolve argument: " + door_name;
} else {
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = false;
display_text_ = "Can you please open " + door_name;
Q_EMIT updateFrameInfo();
/* Wait for the door to be opened */
ros::Rate r(1);
int count = 0;
bool door_open = false;
while (!door_open && count < 30) {
if (sim_auto_door_ && count == 0) {
gh_->openDoor(door_idx);
close_door_idx_ = door_idx;
}
if (as_->isPreemptRequested() || !ros::ok()) { // TODO What about goal not being active or new goal?
ROS_INFO("Preempting action");
as_->setPreempted();
return;
}
door_open = gh_->isDoorOpen(door_idx);
count++;
r.sleep();
}
/* Update the display and send back appropriate fluents */
segbot_logical_translator_gui::ClingoFluent door_open_fluent;
door_open_fluent.args.push_back(door_name);
if (!door_open) {
display_text_ = "Oh no! The request timed out. Let me think...";
Q_EMIT updateFrameInfo();
door_open_fluent.op = "-open";
resp.success = false;
resp.status = "User unable to open door";
} else {
display_text_ = "Thank you for opening the door!";
Q_EMIT updateFrameInfo();
door_open_fluent.op = "open";
resp.success = true;
}
resp.observable_fluents.push_back(door_open_fluent);
}
} else if (req->command.op == "sense") {
std::string sense_fluent_op = req->sense_fluent.op;
if (sense_fluent_op == "open") {
std::string door_name = req->sense_fluent.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
segbot_logical_translator_gui::ClingoFluent door_open;
door_open.args.push_back(door_name);
if (gh_->isDoorOpen(door_idx)) {
door_open.op = "open";
resp.observable_fluents.push_back(door_open);
} else {
door_open.op = "-open";
resp.observable_fluents.push_back(door_open);
}
resp.success = true;
} else if (sense_fluent_op == "beside" || sense_fluent_op == "facing") {
std::string door_name = req->sense_fluent.args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
bwi_mapper::Point2f robot_loc(robot_x_, robot_y_);
bool op_door = (sense_fluent_op == "beside") ?
handler_->isRobotBesideDoor(robot_loc, robot_yaw_, 1.5, door_idx) :
handler_->isRobotFacingDoor(robot_loc, robot_yaw_, 1.5, door_idx);
if (!op_door) {
segbot_logical_translator_gui::ClingoFluent n_op;
n_op.op = "-" + sense_fluent_op;
n_op.args.push_back(door_name);
resp.observable_fluents.push_back(n_op);
} else {
segbot_logical_translator_gui::ClingoFluent op;
op.op = sense_fluent_op;
op.args.push_back(door_name);
resp.observable_fluents.push_back(op);
}
resp.success = true;
} else if (sense_fluent_op == "ploc") {
person_name_ = req->sense_fluent.args[0];
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = true;
display_text_ = "Can you tell me where " + person_name_ + " is?";
location_received_ = false;
Q_EMIT updateFrameInfo();
/* Wait for the location to be received */
ros::Rate r(10);
int count = 0;
bool door_open = false;
while (!location_received_ && count < 300) {
if (as_->isPreemptRequested() || !ros::ok()) { // TODO What about goal not being active or new goal?
ROS_INFO("Preempting action");
as_->setPreempted();
return;
}
count++;
r.sleep();
}
if (location_received_) {
display_text_ = "Thank you! I will try and find " + person_name_ + " in " + person_location_;
segbot_logical_translator_gui::ClingoFluent person_loc_fluent;
person_loc_fluent.op = "inside";
person_loc_fluent.args.push_back(person_name_);
person_loc_fluent.args.push_back(person_location_);
resp.observable_fluents.push_back(person_loc_fluent);
} else {
display_text_ = "Oh no! The request timed out. Let me think...";
}
location_box_enabled_ = false;
Q_EMIT updateFrameInfo();
}
} else if (req->command.op == "evaluate") {
resp.plan_cost = 0;
resp.success = true;
bwi_mapper::Point2f prev_pt(robot_x_, robot_y_), approach_pt;
float prev_yaw = robot_yaw_, approach_yaw = 0;
for (size_t i = 0; i < req->evaluate_fluents.size(); ++i) {
if (req->evaluate_fluents[i].op == "approach") {
std::string door_name = req->evaluate_fluents[i].args[0];
std::cout << "door_name: " << door_name << std::endl;
size_t door_idx = handler_->getDoorIdx(door_name);
bool door_approachable = handler_->getApproachPoint(door_idx,
prev_pt, approach_pt, approach_yaw);
if (!door_approachable) {
resp.plan_cost = std::numeric_limits<float>::max();
resp.success = false;
resp.status = "unable to evaluate one approach leg of plan.";
break;
}
resp.plan_cost += handler_->getPathCost(prev_pt, prev_yaw, approach_pt, approach_yaw);
} else if (req->evaluate_fluents[i].op == "gothrough") {
std::string door_name = req->evaluate_fluents[i].args[0];
size_t door_idx = handler_->getDoorIdx(door_name);
bool door_approachable = handler_->getThroughDoorPoint(door_idx,
prev_pt, approach_pt, approach_yaw);
if (!door_approachable) {
resp.plan_cost = std::numeric_limits<float>::max();
resp.success = false;
resp.status = "unable to evaluate one gothrough leg of plan.";
std::cout << "from " << prev_pt << " to " << approach_pt << std::endl;
break;
}
resp.plan_cost +=
bwi_mapper::getMagnitude(approach_pt - prev_pt); // don't worry about closed doors for now
} else {
continue;
}
prev_pt = approach_pt;
prev_yaw = approach_yaw;
}
} else if (req->command.op == "greet") {
/* Update the display */
button1_enabled_ = false;
button2_enabled_ = false;
location_box_enabled_ = false;
display_text_ = "Hello " + req->command.args[0] + "!!";
Q_EMIT updateFrameInfo();
segbot_logical_translator_gui::ClingoFluent visiting;
visiting.op = "visiting";
visiting.args.push_back(req->command.args[0]);
resp.observable_fluents.push_back(visiting);
resp.success = true;
} else if (req->command.op == "noop") {
resp.success = true;
senseDoorProximity(resp.observable_fluents);
} else if (req->command.op == "finish") {
ROS_INFO("RECEIVED FINISH");
resp.success = true;
prev_door_idx_ = -1; // Robot may be teleported
ce_->finalizeEpisode();
}
// Get location
size_t location_idx =
handler_->getLocationIdx(bwi_mapper::Point2f(robot_x_, robot_y_));
if (location_idx == (size_t) -1) {
ROS_ERROR("Unable to compute position");
} else {
std::string at_str = handler_->getLocationString(location_idx);
segbot_logical_translator_gui::ClingoFluent at;
at.op = "at";
at.args.push_back(at_str);
resp.observable_fluents.push_back(at);
}
/* boost::this_thread::sleep(boost::posix_time::seconds(1)); */
if (resp.success) {
as_->setSucceeded(resp, resp.status);
} else {
as_->setAborted(resp, resp.status);
}
}