void TiledBeizerCurvePrivate::findPoints(float t0, float t1, std::list<Vector>& points, std::list<Vector>::iterator& i)
{
float tMid = (t0 + t1) / 2;
Vector dl, dr, dc;
Vector p0 = calcBezierPoint(t0, dl);
Vector p1 = calcBezierPoint(t1, dr);
if (texture && ((p0 - p1).length() < texture->width()))
{
return;
}
//Vector leftDirection = (p0 - pMid).norm();
//Vector rightDirection = (p1 - pMid).norm();
Vector pMid = calcBezierPoint(tMid, dc);
dl = dl.norm();
dr = dr.norm();
dc = dc.norm();
if (fabs(dl * dc) < ANGLE_TRESHOLD)
findPoints(t0, tMid, points, i);
points.insert(i, pMid);
if (fabs(dr * dc) < ANGLE_TRESHOLD)
findPoints(tMid, t1, points, i);
}
//光标回调函数
void findCircleParameter::onMouse(int event, int x, int y, int, void* params)
{
Mat src = image.clone();
cv::Point pt(x, y);
switch (event)
{
case cv::EVENT_LBUTTONDOWN:
cv::circle(src, pt, src.cols*0.005, cv::Scalar(0, 255, 0), -1);
cv::imshow(check_win_name, src);
points.push_back(pt);
if (2 == points.size())
{
findPoints(center, radius, points);
std::vector<Point>::iterator it = points.begin();
while (it != points.end() - 1)
{
cv::circle(src, *it, src.cols*0.003, cv::Scalar(0, 0, 255), -1);
cv::circle(src, *(it + 1), src.cols*0.003, cv::Scalar(0, 0, 255), -1);
line(src, *it, *(it + 1), Scalar(0, 255, 255), src.cols*0.002);
++it;
}
cv::imshow(check_win_name, src);
lines.push_back(points);
points.clear();
}
break;
case cv::EVENT_MOUSEMOVE:
//cout << "(x, y) = (" << x << ", " << y << ")" << endl;
default:
;
}
}
void PartialSolver::solve()
{
while(!puzzles.empty())
{
Puzzle& front = puzzles.front();
///Clear points to find points for next puzzle
points.clear();
findPoints(front.puzzle);
list< pair<int,int> >::iterator it = points.begin();
for(;it!=points.end(); it++)
{
int row = (*it).first;
int col = (*it).second;
Puzzle back = Puzzle(front);
try
{
back.set(row, col, variable);
back.solve();
if(back.complete())
{
if(back.correct())
{
solved.push_back(back);
}
else
{
incorrect.push_back(back);
}
}
else
{
///If the puzzle is still partially correct continue search
if(partiallyCorrect(back))
puzzles.push_back(back);
}
} catch (bool& value) {
//incorrect.push_back(back);
}
}
puzzles.pop_front();
}
}
void KeypointTracker::loadNewFrame(const Mat& img){
if(!curr_kpts.empty()){
prev_kpts=curr_kpts;
curr_desc.copyTo(prev_desc);
}
findKeypoints(img);
if (!prev_kpts.empty() && !curr_kpts.empty()){
desc_matcher->match(curr_desc, prev_desc, matches);
findPoints();
}
if(!curr_pts.empty()){
prev_pts=curr_pts;
prev_labels=curr_labels;
trackOpticalFlow(img);
}
if (!kpt_pts.empty()){
addTrackedPoints(kpt_pts);
}
img.copyTo(prev_img);
}
void my_plugin_run(unsigned char *frame)
{
short threshold_n_points = 25;
int tot_x=0;
int tot_y=0;
int x_avg = 0;
int y_avg = 0;
//magical scaling needed in order to calibrate opt flow angles to imu angles
short scalex = 1024; //1024*(1/0.75)
short scaley = 1024; //1024*(1/0.76)
int i;
if(old_img_init == 1)
{
memcpy(prev_frame,frame,imgHeight*imgWidth*2);
old_img_init = 0;
}
// ***********************************************************************************************************************
// (1) possibly find new points - keeping possible old ones (normal cv methods / efficient point finding / active corners)
// ***********************************************************************************************************************
if(ALWAYS_NEW_POINTS)
{
// Clear corners
memset(flow_points,0,sizeof(flowPoint)*flow_point_size);
findPoints(gray_frame, frame, imgWidth, imgHeight, &count, max_count, MAX_COUNT, flow_points, &flow_point_size, detected_points);
}
else
{
if(flow_point_size < threshold_n_points)
{
findPoints(gray_frame, frame, imgWidth, imgHeight, &count, max_count, MAX_COUNT, flow_points, &flow_point_size, detected_points);
}
}
// **********************************************************************************************************************
// (2) track the points to the new image, possibly using external information (TTC, known lateral / rotational movements)
// **********************************************************************************************************************
if(count)
{
trackPoints(frame, prev_frame, imgWidth, imgHeight, &count, max_count, MAX_COUNT, flow_points, &flow_point_size, detected_points, x, y, new_x, new_y, dx, dy, status);
//showFlow(frame, x, y, status, count, new_x, new_y, imgWidth, imgHeight);
for (i=0; i<count;i++)
{
// dx[i] = (new_x[i]-x[i]);
// dy[i] = (new_y[i]-y[i]);
dx[i] = flow_points[i].dx;
dy[i] = flow_points[i].dy;
tot_x = tot_x + dx[i];
tot_y = tot_y + dy[i];
}
// using moving average to filter out the noise
if(count)
{
x_buf[buf_point] = (tot_x*scalex)/count;
y_buf[buf_point] = (tot_y*scaley)/count;
buf_point = (buf_point+1) %5;
}
for (i=0;i<5;i++) {
x_avg+=x_buf[i];
y_avg+=y_buf[i];
}
//raw optic flow (for telemetry purpose)
opt_angle_x_raw = x_avg;
opt_angle_y_raw = y_avg;
//tele purpose
//int diff_roll, diff_pitch, mean_alt;
//float mean_tti, median_tti, d_heading, d_pitch, pu[3], pv[3], divergence_error;
/*int USE_FITTING = 0;
if(USE_FITTING == 1)
{
analyseTTI(&divergence, x, y, dx, dy, n_inlier_minu, n_inlier_minv, count, imgWidth, imgHeight);
}*/
memcpy(prev_frame,frame,imgHeight*imgWidth*2);
showFlow(frame, x, y, status, count, new_x, new_y, imgWidth, imgHeight);
//DOWNLINK_SEND_OPTIC_FLOW(DefaultChannel, DefaultDevice, &FPS, &opt_angle_x_raw, &opt_angle_y_raw, &opt_trans_x, &opt_trans_y, &diff_roll, &diff_pitch, &mean_alt, &count, &count, &divergence, &mean_tti, &median_tti, &d_heading, &d_pitch, &pu[2], &pv[2], &divergence_error, n_inlier_minu, n_inlier_minv);
}
//DOWNLINK_SEND_OF_ERROR(DefaultChannel, DefaultDevice, &error_corner, &error_opticflow);
// Send to paparazzi
//gst2ppz.ID = 0x0001;
//gst2ppz.counter++; // to keep track of data through ppz communication
}
int main (int argc,
char** argv)
// ---------------------------------------------------------------------------
// Driver.
// ---------------------------------------------------------------------------
{
char *session, *dump, *points = 0;
int_t NP, NZ, NEL;
int_t np, nel, ntot;
int_t i, j, k, nf, step;
ifstream fldfile;
istream* pntfile;
FEML* F;
Mesh* M;
real_t c, time, timestep, kinvis, beta;
vector<real_t> r, s;
vector<Point*> point;
vector<Element*> elmt;
vector<Element*> Esys;
vector<AuxField*> u;
// -- Initialize.
Femlib::initialize (&argc, &argv);
getargs (argc, argv, session, dump, points);
fldfile.open (dump, ios::in);
if (!fldfile) message (prog, "no field file", ERROR);
// -- Set up 2D mesh information.
F = new FEML (session);
M = new Mesh (F);
NEL = M -> nEl();
NP = Femlib::ivalue ("N_P");
NZ = Femlib::ivalue ("N_Z");
Geometry::set (NP, NZ, NEL, Geometry::Cartesian);
Esys.resize (NEL);
for (k = 0; k < NEL; k++) Esys[k] = new Element (k, NP, M);
// -- Construct the list of points, then find them in the Mesh.
if (points) {
pntfile = new ifstream (points);
if (pntfile -> bad())
message (prog, "unable to open point file", ERROR);
} else
pntfile = &cin;
np = nel = ntot = 0;
loadPoints (*pntfile, np, nel, ntot, point);
findPoints (point, Esys, elmt, r, s);
// -- Load field file, interpolate within it.
cout.precision (8);
while (getDump (fldfile, u, Esys, NP, NZ, NEL,
step, time, timestep, kinvis, beta)) {
if (np) putHeader (session, u, np, NZ, nel,
step, time, timestep, kinvis, beta);
nf = u.size();
for (k = 0; k < NZ; k++)
for (i = 0; i < ntot; i++) {
for (j = 0; j < nf; j++) {
if (elmt[i]) c = u[j] -> probe (elmt[i], r[i], s[i], k);
else c = 0.0;
cout << setw(15) << c;
}
if (verbose && !((i + 1)% nreport))
cerr
<< "interp: plane " << k + 1 << ", "
<< i + 1 << " points interpolated" << endl;
cout << endl;
}
}
Femlib::finalize();
return EXIT_SUCCESS;
}
int main(int argc, char **argv)
{
int cellWidth = -1;
int cellHeight = -1;
int rows = -1;
int cols = -1;
opterr = 0;
int c;
bool help = false;
while((c = getopt(argc, argv, "x:y:r:c:h")) != -1)
{
fprintf(stderr, "%c\n", c);
switch(c)
{
case 'h':
help = true;
break;
case 'x':
cellWidth = atoi(optarg);
break;
case 'y':
cellHeight = atoi(optarg);
break;
case 'r':
rows = atoi(optarg);
break;
case 'c':
cols = atoi(optarg);
break;
case '?':
if(strchr("xyrc", optopt))
{
fprintf(stderr, "Option -%c requires an argument. See help for details.\n", optopt);
return -1;
} else
{
fprintf(stderr, "Unknown option: %c\n", optopt);
return -1;
}
break;
}
}
if(help || optind != argc-1 || argc <= 1 || (cellWidth <= 0 && cols <= 0) || (cellHeight <= 0 && rows <= 0))
{
fprintf(stderr, "Usage:\n\tVertexScanner -r <rows> -c <cols> <png-file>\n");
return -1;
}
ImageDesc img;
loadPNG(argv[optind], &img);
if(cols > 0 && cellWidth <= 0)
cellWidth = img.width/cols;
if(cellWidth > 0 && cols <= 0)
cols = img.width/cellWidth;
if(rows > 0 && cellHeight <= 0)
cellHeight = img.height/rows;
if(cellHeight > 0 && rows <= 0)
rows = img.height/cellHeight;
if(cellWidth*cols > img.width)
{
fprintf(stderr, "Invalid width: %d (image width is %d)\n",
int(cellWidth*cols),
int(img.width));
return -1;
}
if(cellHeight*rows > img.height)
{
fprintf(stderr, "Invalid height: %d (image height is %d)\n",
int(cellHeight*rows),
int(img.height));
return -1;
}
for(int cy=0; cy<rows; cy++)
{
for(int cx=0; cx<cols; cx++)
{
ImageDesc cell;
cell.width = cellWidth;
cell.height = cellHeight;
// the image is a top-down image
cell.pitch = img.pitch;
// so data will point to the last row
cell.data = img.data;
// also offset it by the coordinates of the cell
cell.data += (cx*cellWidth*4)+(cy*cellHeight*img.pitch);
Vec2Array points;
points.points = NULL;
findPoints(&cell, &points);
printf("\n// Cell %d %d\n", cx, cy);
if(points.count > 0)
{
for(int i=0; i<points.count; i++)
{
Vec2 p = points.points[i];
p.x -= cellWidth*0.5f;
p.y -= cellHeight*0.5f;
printf("%g %g\n", p.x, p.y);
}
}
if(points.points)
free(points.points);
}
}
}
void FuncTest::findPointsTest()
{
std::vector<std::vector<int> > pairs;
for (int i = 0; i < 5; i++) {
std::vector<int> list;
pairs.push_back(list);
}
pairs[0].push_back(1);
pairs[0].push_back(2);
pairs[1].push_back(0);
pairs[1].push_back(2);
pairs[2].push_back(1);
pairs[2].push_back(3);
pairs[2].push_back(4);
pairs[3].push_back(2);
pairs[3].push_back(4);
pairs[4].push_back(0);
int idx = 1;
int arr[] = {0, 2, 1, 3, 4};
std::vector<int> res0(arr, arr + sizeof(arr) / sizeof(int));
std::vector<int> res = findPoints(pairs, idx);
QCOMPARE(res0, res);
for (int i = 0; i < 5; i++) {
pairs[i].clear();
}
pairs[0].push_back(0);
pairs[0].push_back(2);
pairs[1].push_back(3);
pairs[2].push_back(2);
pairs[2].push_back(0);
int arr2[] = {3};
res0 = std::vector<int>(arr2, arr2 + sizeof(arr2) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
for (int i = 0; i < 5; i++) {
pairs[i].clear();
}
pairs[0].push_back(1);
pairs[1].push_back(2);
pairs[2].push_back(0);
idx = 0;
int arr3[] = {1, 2, 0};
res0 = std::vector<int>(arr3, arr3 + sizeof(arr3) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
for (int i = 0; i < 5; i++) {
pairs[i].clear();
}
pairs[0].push_back(2);
pairs[1].push_back(2);
pairs[1].push_back(0);
pairs[2].push_back(0);
pairs[2].push_back(1);
pairs[3].push_back(4);
pairs[4].push_back(3);
idx = 2;
int arr5[] = {0, 1, 2};
res0 = std::vector<int>(arr5, arr5 + sizeof(arr5) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
idx = 3;
int arr4[] = {4, 3};
res0 = std::vector<int>(arr4, arr4 + sizeof(arr4) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
}