void Node::computeInliersAndError(const std::vector<cv::DMatch>& matches,
const Eigen::Matrix4f& transformation,
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& earlier,
std::vector<cv::DMatch>& inliers, //output var
double& mean_error,
vector<double>& errors,
double squaredMaxInlierDistInM) const{ //output var
std::clock_t starttime=std::clock();
inliers.clear();
errors.clear();
vector<pair<float,int> > dists;
std::vector<cv::DMatch> inliers_temp;
assert(matches.size() > 0);
mean_error = 0.0;
for (unsigned int j = 0; j < matches.size(); j++){ //compute new error and inliers
unsigned int this_id = matches[j].queryIdx;
unsigned int earlier_id = matches[j].trainIdx;
Eigen::Vector4f vec = (transformation * origins[this_id]) - earlier[earlier_id];
double error = vec.dot(vec);
if(error > squaredMaxInlierDistInM)
continue; //ignore outliers
if(!(error >= 0.0)){
ROS_ERROR_STREAM("Transformation for error !> 0: " << transformation);
ROS_ERROR_STREAM(error << " " << matches.size());
}
error = sqrt(error);
dists.push_back(pair<float,int>(error,j));
inliers_temp.push_back(matches[j]); //include inlier
mean_error += error;
errors.push_back(error);
}
if (inliers_temp.size()<3){ //at least the samples should be inliers
ROS_WARN_COND(inliers_temp.size() > 3, "No inliers at all in %d matches!", matches.size()); // only warn if this checks for all initial matches
mean_error = 1e9;
} else {
mean_error /= inliers_temp.size();
// sort inlier ascending according to their error
sort(dists.begin(),dists.end());
inliers.resize(inliers_temp.size());
for (unsigned int i=0; i<inliers_temp.size(); i++){
inliers[i] = matches[dists[i].second];
}
}
if(!(mean_error>0)) ROS_ERROR_STREAM("Transformation for mean error !> 0: " << transformation);
if(!(mean_error>0)) ROS_ERROR_STREAM(mean_error << " " << inliers_temp.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", __FUNCTION__ << " runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
bool TaskEventDetector::detect(task_recorder2_msgs::DetectedEvents::Request& request,
task_recorder2_msgs::DetectedEvents::Response& response)
{
mutex_.lock();
last_data_labels_.clear();
last_data_samples_.clear();
num_labels_ = request.num_data_samples;
save_data_samples_ = request.save_data_samples;
bool has_been_detecting = detecting_;
detecting_ = true;
mutex_.unlock();
sync_mutex_.lock();
data_labels_ready_ = false;
sync_mutex_.unlock();
// gather data
boost::unique_lock<boost::mutex> lock(sync_mutex_);
while (!data_labels_ready_)
{
cond_.wait(lock);
}
response.labels = last_data_labels_;
if(!computeLabel(request.method, response.labels, response.label))
{
response.return_code = task_recorder2_msgs::DetectedEvents::Response::SERVICE_CALL_FAILED;
response.info.assign("Could not compute label.");
return true;
}
mutex_.lock();
detecting_ = has_been_detecting;
save_data_samples_ = false;
mutex_.unlock();
if(request.save_data_samples)
{
ROS_WARN_COND(request.num_data_samples != (int)last_data_samples_.size(),
"Number of data samples >%i< does not correspond to number of recorded samples >%i<. Number of labels is >%i<.",
request.num_data_samples, (int)last_data_samples_.size(), (int)last_data_labels_.size());
// offset time stamps
if(last_data_samples_.empty())
{
response.return_code = task_recorder2_msgs::DetectedEvents::Response::SERVICE_CALL_FAILED;
response.info.assign("No data samples recorded. Cannot save last data samples.");
return true;
}
ros::Time start_time = last_data_samples_[0].header.stamp;
for (int i = 0; i < (int)last_data_samples_.size(); ++i)
{
last_data_samples_[i].header.stamp = static_cast<ros::Time> (ros::TIME_MIN + (last_data_samples_[i].header.stamp - start_time));
}
ROS_VERIFY(usc_utilities::FileIO<task_recorder2_msgs::DataSample>::writeToBagFileWithTimeStamps(last_data_samples_, DATA_SAMPLE_TOPIC_NAME, LAST_DATA_SAMPLES_BAG_FILE_NAME));
ROS_VERIFY(usc_utilities::FileIO<task_recorder2_msgs::Description>::writeToBagFileWithTimeStamp(request.description, ros::TIME_MIN, DESCRIPTION_TOPIC_NAME, LAST_DESCRIPTION_BAG_FILE_NAME));
}
response.return_code = task_recorder2_msgs::DetectedEvents::Response::SERVICE_CALL_SUCCESSFUL;
response.info.assign("Detection result is " + boost::lexical_cast<std::string>(response.label.binary_label.label) + ".");
return true;
}
vector<cv::Point2f> FeatureTracker::undistortedPoints(std::vector<cv::Point2f> v)
{
vector<cv::Point2f> un_pts;
for (unsigned int i = 0; i < v.size(); i++)
{
Eigen::Vector2d a(v[i].x, v[i].y),re_pro;
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
//test J
/*
Eigen::Vector2d uv_b,uv_bd;
Eigen::Vector3d d;
Eigen::Matrix<double,2,3> J,JD;
m_camera->spaceToPlane(b, uv_b , J);
ROS_INFO_STREAM("x "<<b.transpose());
for( int i = 0 ;i <= 2 ;i++)
{
d<<0,0,0;
d(i) = 0.0000001;
m_camera->spaceToPlane(b+d,uv_bd);
ROS_INFO_STREAM("dx "<<d.transpose());
ROS_INFO_STREAM("f(x+dx) "<<uv_bd.transpose());
ROS_INFO_STREAM("df(x) "<<(uv_bd - uv_b).transpose());
JD(0,i) = (uv_bd - uv_b)(0)/0.0000001;
JD(1,i) = (uv_bd - uv_b)(1)/0.0000001;
}
ROS_INFO_STREAM("J "<<J);
ROS_INFO_STREAM("JD "<<JD);
ROS_INFO_STREAM(" ");
*/
/*
ROS_INFO("a b re_pro J");
ROS_INFO_STREAM(a.transpose());
ROS_INFO_STREAM(b.transpose());
ROS_INFO_STREAM(re_pro.transpose());
ROS_INFO_STREAM(J);
cout<<"a "<< a.transpose()<<endl;
cout<<"b "<<b.transpose()<<endl;
cout<<"re_pro"<<re_pro.transpose()<<endl;
cout<<"J "<<endl<<J<<endl<<endl;
*/
//m_camera->liftSphere(a, c);
un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
if(b.z()!=b.z())
{
ROS_WARN_COND(b.z()!=b.z(),"nan false");
//ROS_BREAK();
}
//cout<<"Projective "<<"x "<<b.x() <<" y "<<b.y()<<" z "<<b.z()<<endl;
//cout<<"Sphere "<<"x "<<c.x() <<" y "<<c.y() <<" z "<<c.z()<<endl;
/*
for(int i = 1 ; i < ROW ; i=i+4)
for(int j = 1 ; j < COL ; j=j+4)
{
Eigen::Vector2d c(i,j);
Eigen::Vector3d d;
m_camera->liftProjective(c, d);
cout<<"u "<<i<<" v "<<j<<endl;
cout<<"x "<<d.x() <<" y "<<d.y() <<" z "<<d.z()<<endl;
cout<<"x "<<d.x() / d.z() <<" y "<<d.y() / d.z()<<endl;
if(d.z()!=d.z())
{
cout<<"false"<<endl;
}
}
ROS_BREAK();
*/
}
return un_pts;
}