void FeatureTracker::rejectWithF()
{
if (forw_pts.size() >= 8)
{
ROS_DEBUG("FM ransac begins");
TicToc t_f;
vector<cv::Point2f> un_prev_pts(prev_pts.size()), un_forw_pts(forw_pts.size());
for (unsigned int i = 0; i < prev_pts.size(); i++)
{
Eigen::Vector3d tmp_p;
m_camera->liftProjective(Eigen::Vector2d(prev_pts[i].x, prev_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_prev_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
}
vector<uchar> status;
cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
int size_a = prev_pts.size();
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
}
}
SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
int ndof;
int contacts;
char lagrangianModelName[20] = "";
getLagrangianModelName(lagrangianModelName);
getActiveDOFsize(&ndof);
vector<int> activeDOF(ndof);
if (ndof > 0)
getActiveDOF(&(activeDOF[0]));
else
ndof = *NDOF;
///////////////////////////////////////////
// Computation of the Lagrangian model
///////////////////////////////////////////
matrix<double, column_major> M(*NDOF, *NDOF);
vector<double> N(*NDOF);
if (strcmp(lagrangianModelName, "Human36") == 0)
{
double L[31];
double addl[84];
double mass;
GetModelMass(&mass);
GetAnatomicalLengths(L);
GetTag2JointLengths(addl);
InertiaH36(&(M(0, 0)), q, L, addl, mass);
NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
}
else
{
Inertia(&(M(0, 0)), q);
NLEffects(&(N[0]), q, qdot);
}
//////////////////////////////////////////////////
//Additionnal forces
/////////////////////////////////////////////////
vector<double> fAdditionnal(*NDOF);
if (strcmp(lagrangianModelName, "PA10") == 0)
Friction(&(fAdditionnal[0]), q, qdot);
else if (strcmp(lagrangianModelName, "RX90") == 0)
SpringForce(&(fAdditionnal[0]), q);
else
fAdditionnal.clear();
///////////////////////////////////////////////////
// Limitation to the selected degrees of freedom
///////////////////////////////////////////////////
reduceMatrix(M, activeDOF, activeDOF);
N = reduceVector(N, activeDOF);
fAdditionnal = reduceVector(fAdditionnal, activeDOF);
//////////////////////////////////////////////////
// Proportionnel-derivee in the task space
//////////////////////////////////////////////////
vector<double> sDesired(*NDOF);
vector<double> sdotDesired(*NDOF);
matrix<double, column_major> sddotDesiredMATRIX(*NDOF, 1);
matrix_column<matrix<double, column_major> > sddotDesired(sddotDesiredMATRIX, 1);
//vector<double> sddotDesired(*NDOF);
trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);
//v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
vector<double> TF(*NDOF);
vector<double> h(*NDOF);
matrix<double, column_major> H(*NDOF, *NDOF);
TaskFunction(&(TF[0]), q);
TaskJacobian(&(H(0, 0)), q);
TaskNLEffects(&(h[0]), q, qdot);
double Kp = 100.0;
double Kv = 30.0;
vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;
/////////////////////////////////////////////////////////////
// Generalized Torques to make the realisation of the command
/////////////////////////////////////////////////////////////
//qddot = inv(H)*v;
vector<int> ipiv(*NDOF);
// lapack::getrf(H, ipiv);
// sddotDesired = prod(H, sddotDesired);
lapack::gesv(H, ipiv, sddotDesiredMATRIX);
//D = M*qddot(ActiveDOF) + N + FAdditionnal;
sddotDesired = reduceVector(sddotDesired, activeDOF);
N += prod(M, sddotDesired) + fAdditionnal;
//nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
//Torques = zeros(q);
//Torques(ActiveDOF(1:nmot)) = D(1:nmot);
for (int i = 0; i < *NDOF; i++)
torques[i] = 0;
expandVector(torques, N, activeDOF);
}
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
cv::Mat img;
TicToc t_r;
cur_time = _cur_time;
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
if (forw_img.empty())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
forw_pts.clear();
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i < int(forw_pts.size()); i++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);
reduceVector(track_cnt, status);
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
for (auto &n : track_cnt)
n++;
if (PUB_THIS_FRAME)
{
rejectWithF();
ROS_DEBUG("set mask begins");
TicToc t_m;
setMask();
ROS_DEBUG("set mask costs %fms", t_m.toc());
ROS_DEBUG("detect feature begins");
TicToc t_t;
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());
ROS_DEBUG("add feature begins");
TicToc t_a;
addPoints();
ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
}
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
undistortedPoints();
prev_time = cur_time;
}
void FeatureTracker::readImage(const cv::Mat &_img)
{
cv::Mat img;
TicToc t_r;
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
if (forw_img.empty())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
forw_pts.clear();
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
// Status will give us infor whether the feature is being tracked
// Forw_img is the new image and forw_pts are the "KLT" points.
// We will later track more points that are not tracked by KLT
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i < int(forw_pts.size()); i++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
// Resizing vectors so that only the good status (tracked) features are in the vectors
// The remaining features will be tracked/added by KLT
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
if (PUB_THIS_FRAME)
{
rejectWithF();
for (auto &n : track_cnt)
n++;
ROS_DEBUG("set mask begins");
TicToc t_m;
setMask();
ROS_DEBUG("set mask costs %fms", t_m.toc());
ROS_DEBUG("detect feature begins");
TicToc t_t;
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
// If > 0, meaning not all points are tracked, so we will ty
// to track more points
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
// There are features that are not tracked by KLT
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());
ROS_DEBUG("add feature begins");
TicToc t_a;
// The newly added "good features" are assigned -1 ID (will update later).
// These are new observation points
addPoints();
ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
prev_img = forw_img;
prev_pts = forw_pts;
}
cur_img = forw_img;
cur_pts = forw_pts;
}
