bool
Beam2d :: computeGtoLRotationMatrix(FloatMatrix &answer)
{
double sine, cosine;
int ndofs = computeNumberOfGlobalDofs();
answer.resize(ndofs, ndofs);
answer.zero();
sine = sin( this->givePitch() );
cosine = cos(pitch);
answer.at(1, 1) = cosine;
answer.at(1, 2) = sine;
answer.at(2, 1) = -sine;
answer.at(2, 2) = cosine;
answer.at(3, 3) = 1.;
answer.at(4, 4) = cosine;
answer.at(4, 5) = sine;
answer.at(5, 4) = -sine;
answer.at(5, 5) = cosine;
answer.at(6, 6) = 1.;
for ( int i = 7; i <= ndofs; i++ ) {
answer.at(i, i) = 1.0;
}
if ( this->hasDofs2Condense() ) {
int condensedDofCounter = 0;
DofIDItem dofids[] = {
D_u, D_w, R_v
};
FloatMatrix l2p(6, ndofs); // local -> primary
l2p.zero();
// loop over nodes
for ( int inode = 0; inode < 2; inode++ ) {
// loop over DOFs
for ( int idof = 0; idof < 3; idof++ ) {
int eq = inode * 3 + idof + 1;
if ( ghostNodes [ inode ] ) {
if ( ghostNodes [ inode ]->hasDofID(dofids [ idof ]) ) {
condensedDofCounter++;
l2p.at(eq, 6 + condensedDofCounter) = 1.0;
continue;
}
}
l2p.at(eq, eq) = 1.0;
}
}
FloatMatrix g2l(answer);
answer.beProductOf(l2p, g2l);
}
return true;
}
TEST(geodetic2local, north) {
std::unique_ptr<geocon::geodetic2local> g2l(
geocon::geodetic2local::Create(0, 0, 0));
MSP::CCS::CartesianCoordinates cart;
MSP::CCS::Accuracy acc;
g2l->to_local(1, 0, 0, 0, &cart, &acc);
EXPECT_GT(cart.x(), 0.0);
EXPECT_EQ(0.0, cart.y());
EXPECT_GT(cart.z(), 0.0);
}
bool Target::track(const cv::Mat &prev, const cv::Mat &curr, double stamp)
{
// TODO: 应该根据轨迹的方向扩展搜索范围 ...
// FIXME: 简单的四周扩展 ...
// 第一次得到的特征点，总有部分不在活动目标上，所以应该在N帧之后，扔掉这些点，让“跟踪点”真正落在目标上 ...
int exp = 60; // 不知道这个距离是否合理 ...
cv::Rect search_roi = last_rc_;
search_roi.x -= exp;
search_roi.y -= exp;
search_roi.width += 2 * exp;
search_roi.height += 2 * exp;
search_roi &= outer_;
PTS last_pts = layers_.back(), curr_pts, last_pts2;
g2l(last_pts, search_roi.tl());
//cv::Mat status, err, status2, err2;
std::vector<unsigned char> status, err, status2, err2;
//cv::calcOpticalFlowPyrLK(prev(search_roi), curr(search_roi), last_pts, curr_pts, status, err);
calcLKOpticalFlow(prev(search_roi), curr(search_roi), last_pts, curr_pts, status);
//#define REVERSE_FIND
#ifdef REVERSE_FIND
//cv::calcOpticalFlowPyrLK(curr(search_roi), prev(search_roi), curr_pts, last_pts2, status2, err2); // 反向查找 ..
calcLKOpticalFlow(curr(search_roi), prev(search_roi), curr_pts, last_pts2, status2); // 反向查找 ..
#endif //
//for (int r = 0; r < status.rows; r++) {
// // 标记找错的
// if (status.at<uchar>(r, 0) != 1) {
// curr_pts[r].x = -10000;
// }
//}
for (int r = 0; r < status.size(); r++) {
// 标记找错的
if (status[r] != 1) {
curr_pts[r].x = -10000;
}
}
#ifdef REVERSE_FIND
/// 比较 last_pts 与 last_pts2，如果比较接近，说明反向查找成功 ...
for (int i = 0; i < (int)last_pts.size(); i++) {
if (::distance(last_pts[i], last_pts2[i]) > 5.0) {
curr_pts[i].x = -10000;
}
}
#endif
/// 删除错误点对应的轨迹 ...
PTS valid_pts;
for (int i = (int)curr_pts.size() - 1; i >= 0; i--) {
if (curr_pts[i].x < -5000) {
remove_path(i);
}
else {
valid_pts.push_back(curr_pts[i]);
}
}
if ((int)valid_pts.size() < min_pts_) {
return false;
}
std::reverse(valid_pts.begin(), valid_pts.end()); // 需要反序 ...
l2g(valid_pts, search_roi.tl());
layers_.push_back(valid_pts);
last_rc_ = cv::boundingRect(valid_pts);
brc_ |= last_rc_;
return check_paths(stamp);
}
