void corners(T& head, const T & tail, int i, int dim, std::vector<T> & cor) {
if(i < dim) {
head[i] = left(tail[i]);
corners(head, tail, i+1, dim, cor);
head[i] = right(tail[i]);
corners(head, tail, i+1, dim, cor);
head[i] = tail[i];
}
else {
cor.push_back(head);
}
}
void test_corners()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( corners(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( corners(Matrix4d()) );
CALL_SUBTEST_3( corners(Matrix<int,10,12>()) );
CALL_SUBTEST_4( corners(MatrixXcf(5, 7)) );
CALL_SUBTEST_5( corners(MatrixXf(21, 20)) );
CALL_SUBTEST_1(( corners_fixedsize<Matrix<float, 1, 1>, 1, 1, 0, 0>() ));
CALL_SUBTEST_2(( corners_fixedsize<Matrix4d,2,2,1,1>() ));
CALL_SUBTEST_3(( corners_fixedsize<Matrix<int,10,12>,4,7,5,2>() ));
}
}
void TopologyInfo::find_side( EntityTopology topo,
const unsigned* side_vertices,
unsigned num_vertices,
unsigned& dimension_out,
unsigned& number_out,
bool& reversed_out,
MsqError& err )
{
switch (num_vertices) {
case 1:
dimension_out = 0;
number_out = *side_vertices;
reversed_out = false;
if (*side_vertices >= corners(topo))
MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid corner number: %u\n", *side_vertices);
break;
case 2:
dimension_out = 1;
number_out = find_edge( topo, side_vertices, reversed_out, err );
MSQ_CHKERR(err);
break;
case 3:
case 4:
dimension_out = 2;
number_out = find_face( topo, side_vertices, num_vertices, reversed_out, err );
MSQ_CHKERR(err);
break;
default:
MSQ_SETERR(err)(MsqError::UNSUPPORTED_ELEMENT, "Invalid number of side vertices: %u\n", num_vertices );
break;
}
}
std::vector<CvPoint2D32f> findCorners(Image<PixRGB<byte> > &img, int rows, int cols)
{
int count = 0;
std::vector<CvPoint2D32f> corners(rows*cols);
Image<byte> in = luminance(img);
int result = cvFindChessboardCorners(img2ipl(in), cvSize(rows,cols),
&corners[0], &count,
CV_CALIB_CB_ADAPTIVE_THRESH |
CV_CALIB_CB_NORMALIZE_IMAGE |
CV_CALIB_CB_FILTER_QUADS);
// result = 0 if not all corners were found
// Find corners to an accuracy of 0.1 pixel
if(result != 0)
{
cvFindCornerSubPix(img2ipl(in),
&corners[0],
count,
cvSize(10,10), //win
cvSize(-1,-1), //zero_zone
cvTermCriteria(CV_TERMCRIT_ITER,1000,0.01) );
return corners;
} else {
return std::vector<CvPoint2D32f>();
}
}
const unsigned* TopologyInfo::side_vertices( EntityTopology topo,
unsigned dim,
unsigned side,
unsigned& count_out )
{
static const unsigned all[] = { 0, 1, 2, 3, 4, 5, 6, 7 };
const unsigned* result;
if (dim != 0 && dim == dimension(topo))
{
count_out = corners( topo );
result = all;
}
else if (dim == 1)
{
count_out = 2;
result = edge_vertices( topo, side );
}
else if( dim == 2)
{
result = face_vertices( topo, side, count_out );
}
else
{
result = 0;
}
return result;
}
void Text31::request (Requisition& req) const {
_body->request(req);
Requirement& rx = req.x_requirement();
Requirement& ry = req.y_requirement();
Coord left, bottom, right, top;
left = -rx.natural()*rx.alignment();
right = left + rx.natural();
bottom = -ry.natural()*ry.alignment();
top = bottom + ry.natural();
Text31* text = (Text31*) this;
text->_ctrlpts = 4;
text->_x[0] = left;
text->_y[0] = bottom;
text->_x[1] = left;
text->_y[1] = top;
text->_x[2] = right;
text->_y[2] = top;
text->_x[3] = right;
text->_y[3] = bottom;
if (_t != nil) {
corners(left, bottom, right, top, *_t);
}
rx.natural(right - left);
rx.stretch(0.0);
rx.shrink(0.0);
rx.alignment(-left / rx.natural());
ry.natural(top - bottom);
ry.stretch(0.0);
ry.shrink(0.0);
ry.alignment(-bottom / ry.natural());
}
int TopologyInfo::higher_order_from_side( EntityTopology topo,
unsigned num_nodes,
unsigned side_dimension,
unsigned side_number,
MsqError& err )
{
bool mids[4] = { true };
higher_order( topo, num_nodes, mids[1], mids[2], mids[3], err );
MSQ_ERRZERO(err);
if (side_dimension > dimension(topo) ||
side_number > adjacent(topo, side_dimension)) {
MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid side number: %u\n", side_number );
return 0;
}
if (!mids[side_dimension])
return -1;
int result = side_number;
switch (side_dimension) {
case 3: if (mids[2]) result += faces(topo);
case 2: if (mids[1]) result += edges(topo);
case 1: result += corners(topo);
case 0: break;
default:
MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid dimension: %u\n", side_dimension );
return 0;
}
return result;
}
void PolyGraphic::getextent_gs (
Coord& l, Coord& b, Coord& cx, Coord& cy, float& tol, Graphic31* gs
) {
Extent e;
l = b = cx = cy = tol = 0.0;
Graphic31 gstemp;
Transformer ttemp;
Extent te;
gstemp.transformer(&ttemp);
GlyphIndex count = _body->count();
for (GlyphIndex i = 0; i < count; i++) {
Graphic31* gr = (Graphic31*) _body->component(i);
concatgs_(gr, gr, gs, &gstemp);
concatXform_(gr, nil, gr->transformer(), &ttemp);
getextent_(gr, te._left, te._bottom, te._cx, te._cy, te._tol, &gstemp);
e.Merge(te);
}
gstemp.transformer(nil); // to avoid deleting ttemp explicitly
l = e._left; b = e._bottom; cx = l+(e._cx-l)*2.0; cy = b+(e._cy-b)*2.0;
tol = e._tol;
Transformer* tx = gs->transformer();
if (tx != nil) {
corners(l, b, cx, cy, *tx);
}
cx = (cx + l)/2.0;
cy = (cy + b)/2.0;
}
TableRopeScene::TableRopeScene(fs::path ropeFile, bool telekinesis) : GrabbingScene(telekinesis) {
// vector<double> firstJoints = doubleVecFromFile((KNOT_DATA / "init_joints_train.txt").string());
// ValuesInds vi = getValuesInds(firstJoints);
// setupDefaultROSRave();
// pr2m->pr2->setDOFValues(vi.second, vi.first);
vector<int> indices(1,pr2m->pr2->robot->GetJointIndex("torso_lift_joint"));
vector<double> values(1, .31);
pr2m->pr2->setDOFValues(indices, values);
vector<btVector3> tableCornersWorld = toBulletVectors(floatMatFromFile((KNOT_DATA / "table_corners.txt").string())) * METERS;
vector<btVector3> controlPointsWorld = toBulletVectors(floatMatFromFile(ropeFile.string())) * METERS;
PlotPoints::Ptr corners(new PlotPoints(20));
corners->setPoints(tableCornersWorld);
env->add(corners);
m_table = makeTable(tableCornersWorld, .1*GeneralConfig::scale);
float seglen = controlPointsWorld[0].distance(controlPointsWorld[1]);
// m_rope.reset(new CapsuleRope(controlPointsWorld, fmin(seglen/4.1,.0075*METERS)));
m_rope.reset(new CapsuleRope(controlPointsWorld, .0075*METERS));
env->add(m_rope);
env->add(m_table);
setGrabBodies(m_rope->children);
}
const unsigned* TopologyInfo::side_vertices( EntityTopology topo,
unsigned dim,
unsigned side,
unsigned& count_out,
MsqError& err )
{
static const unsigned all[] = { 0, 1, 2, 3, 4, 5, 6, 7 };
const unsigned* result;
if (dim != 0 && dim == dimension(topo))
{
count_out = corners( topo );
result = all;
}
else if (dim == 1)
{
count_out = 2;
result = edge_vertices( topo, side, err );
}
else if( dim == 2)
{
result = face_vertices( topo, side, count_out, err );
}
else
{
MSQ_SETERR(err)(MsqError::INVALID_ARG);
count_out = 0;
result = 0;
}
return result;
}
/**
* Reimplemented form UMLWidget::paint to draw
* ClassifierWidget.
*/
void ClassifierWidget::paint(QPainter *painter, const QStyleOptionGraphicsItem*, QWidget *)
{
QPen pen(lineColor(), lineWidth());
if (shouldDrawAsCircle()) {
painter->setPen(pen);
painter->setBrush(brush());
// m_classifierRect represents circle geometry when shouldDrawAsCircle is true.
painter->drawEllipse(m_classifierRect);
}
else {
// The elements not to be drawn will have null dimension and
// hence it effectively is not drawn. (automatic ;) )
painter->setPen(pen);
painter->setBrush(brush());
painter->drawRoundedRect(m_classifierRect, 2, 2);
painter->drawLines(m_classifierLines, 2);
painter->setPen(QPen(Qt::NoPen));
painter->setBrush(awesomeHeaderBrush());
Uml::Corners corners(Uml::corner_TopLeft | Uml::corner_TopRight);
Widget_Utils::drawRoundedRect(painter, textItemGroupAt(HeaderGroupIndex)->groupGeometry(),
2, 2, corners);
pen.setStyle(Qt::DotLine);
painter->setPen(pen);
painter->setBrush(brush());
painter->drawRect(m_templateRect);
}
}
bool FieldLineDetector::findCornerMapping(vector<FieldCorner*> &cornerMapping)
{
vector<FieldCorner*> corners(detectedCorners);
sort(corners.begin(), corners.end(), [](FieldCorner* p1, FieldCorner* p2)
{
return (p1->point.y < p2->point.y);
});
// check current mapping, if available
if (!cornerMapping.empty())
{
if (findCornerMappingRec(corners, cornerMapping))
{
return true;
}
}
for (int i = 0; i < corners.size(); i++)
{
cornerMapping.clear();
cornerMapping.push_back(corners.at(i));
if (findCornerMappingRec(corners, cornerMapping))
{
return true;
}
}
cornerMapping.clear();
return false;
}
Float
simplex_noise<Float,N>::sample(
vector const &in)
{
Float res = static_cast<Float>(0);
vector tmp = stretch_m() * in;
for(typename vector::iterator it = tmp.begin(); it != tmp.end(); ++it)
{
*it = std::floor(*it);
}
vector floored(tmp);
tmp = inv_m() * tmp;
tmp = in - tmp;
vector offset(tmp);
tmp = stretch_m() * tmp;
corner_array c = corners(tmp);
for (typename corner_array::const_iterator v = c.begin(); v != c.end(); ++v)
{
vector t(in - inv_m() * (floored + *v));
res +=
contrib(t, floored + *v);
}
// FIXME: replace this magic number with something sensible
return static_cast<Float>(40.0) * res;
}
void Proc3DSCropper::projection(const cv::Mat &input, cv::Mat &output, const int in_width, const int in_height,
const int out_width, const int out_height, const bool hflip, const bool vflip) {
double width_rate = in_width/ double(this->width);
double height_rate = in_height / double(this->height);
if(this->corners.size() != 4) {
cv::resize(input, output, cv::Size(out_width, out_height));
return;
}
std::vector<cv::Point2f> corners(4);
for (int index = 0; index < 4; ++index) {
const cv::Point2f& op = this->corners[index];
std::cout << "point: " << op << std::endl;
cv::Point2f p(op.x * width_rate, op.y * height_rate);
corners[index] = p;
}
std::vector<cv::Point2f> dist_corners(4);
int y1 = 0;
int y2 = out_height;
int x1 = 0;
int x2 = out_width;
if(hflip) std::swap(x1, x2);
if(vflip) std::swap(y1, y2);
dist_corners[0] = cv::Point2f(x1, y1);
dist_corners[1] = cv::Point2f(x1, y2);
dist_corners[2] = cv::Point2f(x2, y2);
dist_corners[3] = cv::Point2f(x2, y1);
cv::Mat homogrp = cv::findHomography(corners, dist_corners, 0);
cv::warpPerspective(input, output, homogrp, cv::Size(out_width, out_height));
return;
};
// ---------------------------------------------------------
void
NodeMGInterp::define(const DisjointBoxLayout& a_grids,
int a_numcomps,
int a_refRatio,
const ProblemDomain& a_domain)
{
m_refRatio = a_refRatio;
m_domain = a_domain;
m_grids = a_grids;
m_boxRef = Box(IntVect::Zero, (m_refRatio-1)*IntVect::Unit);
Box corners(IntVect::Zero, IntVect::Unit);
m_weights.define(corners, m_boxRef.numPts());
FORT_NODEINTERPMG_GETWEIGHTS(CHF_CONST_INT(m_refRatio),
CHF_BOX(m_boxRef),
CHF_FRA(m_weights));
// create the work array
DisjointBoxLayout coarsenedGrids;
coarsen(coarsenedGrids, a_grids, m_refRatio);
m_coarsenedFine.define(coarsenedGrids, a_numcomps);
is_defined = true;
}
void CoordinateTransform::initPerspective()
{
std::vector<cv::Point2f> corners(4);
std::vector<cv::Point2f> corners_trans(4);
//选定变换前后的四个对应点
//此点为基准点：距摄像头纵向距离为254cm，距摄像头光轴距离为-60cm
corners[0]=cv::Point2f(321,973);
corners[1]=cv::Point2f(1627,973);
corners[2]=cv::Point2f(883,240);
corners[3]=cv::Point2f(1065,240);
//1pix=1cm
corners_trans[0]=cv::Point2f(960-120,1080-254);
corners_trans[1]=cv::Point2f(960+120,1080-254);
corners_trans[2]=cv::Point2f(960-120,1080-2004);
corners_trans[3]=cv::Point2f(960+120,1080-2004);
//得到透视变换矩阵
transMat_=cv::getPerspectiveTransform(corners,corners_trans);
inv_transMat_=cv::getPerspectiveTransform(corners_trans,corners);
std::cout<<"transMat_:"<<transMat_<<std::endl;
std::cout<<"inv_transMat_:"<<inv_transMat_<<std::endl;
cv::FileStorage fs("/home/lzp/workspace/CoordinateTransfoarm/camera.yml",cv::FileStorage::APPEND);
fs<<"transMat_"<<transMat_;
fs<<"inv_transMat_"<<inv_transMat_;
fs.release();
}
void HDS_Face::scaleDown()
{
int n = 0;
for (auto v : corners()) {
v->pos = scaledCorners.at(n);
n++;
}
}
shared_ptr<Grid> GridFactory::createGridFromConnectivityArrays(
const GridParameters ¶ms, const arma::Mat<double> &vertices,
const arma::Mat<int> &elementCorners,
const std::vector<int> &domainIndices) {
const int dimGrid = 2, dimWorld = 3;
if (params.topology != GridParameters::TRIANGULAR)
throw std::invalid_argument("createGridFromConnectivityArrays(): "
"unsupported grid topology");
if (vertices.n_rows != dimWorld)
throw std::invalid_argument("createGridFromConnectivityArrays(): "
"the 'vertices' array "
"must have exactly 3 rows");
if (elementCorners.n_rows < 3)
throw std::invalid_argument("createGridFromConnectivityArrays(): "
"the 'elementCorners' array "
"must have at least 3 rows");
if (!domainIndices.empty() && domainIndices.size() != elementCorners.n_cols)
throw std::invalid_argument(
"createGridFromConnectivityArrays(): "
"'domainIndices' must either be empty or contain as many "
"elements as 'elementCorners' has columns");
shared_ptr<Dune::GridFactory<Default2dIn3dDuneGrid>> factory(
new Dune::GridFactory<Default2dIn3dDuneGrid>());
for (size_t i = 0; i < vertices.n_cols; ++i) {
Dune::FieldVector<double, dimWorld> v;
v[0] = vertices(0, i);
v[1] = vertices(1, i);
v[2] = vertices(2, i);
factory->insertVertex(v);
}
const GeometryType type(GeometryType::simplex, dimGrid);
const size_t vertexCount = vertices.n_cols;
std::vector<unsigned int> corners(3);
for (size_t i = 0; i < elementCorners.n_cols; ++i) {
if (elementCorners(0, i) < 0 || elementCorners(0, i) >= vertexCount ||
elementCorners(1, i) < 0 || elementCorners(1, i) >= vertexCount ||
elementCorners(2, i) < 0 || elementCorners(2, i) >= vertexCount)
throw std::invalid_argument("createGridFromConnectivityArrays(): invalid "
"vertex index in element #" +
toString(i));
corners[0] = elementCorners(0, i);
corners[1] = elementCorners(1, i);
corners[2] = elementCorners(2, i);
factory->insertElement(type, corners);
}
shared_ptr<Grid> result;
if (domainIndices.empty())
result.reset(new Default2dIn3dGrid(factory,
GridParameters::TRIANGULAR));
else
result.reset(
new Default2dIn3dGrid(factory,GridParameters::TRIANGULAR,
domainIndices));
return result;
}
std::vector<typename SetType::VectorType> getCorners(const SetType & set) {
typedef typename SetType::VectorType VectorType;
std::vector<VectorType> cor;
VectorType v = set.get_r();
corners(v, set.get_r(), 0, v.dimension(), cor);
for(typename std::vector<VectorType>::iterator it = cor.begin(); it != cor.end(); ++it){
*it = set.get_x() + set.get_C() * set.get_r0() + set.get_B() * *it;
}
return cor;
}
QVector3D HDS_Face::center() const
{
auto cs = corners();
QVector3D c;
for(auto p : cs) {
c += p->pos;
}
c /= (qreal) cs.size();
return c;
}
// Return the rotation matrices for each rotation
void rotate(cv::Mat& src, double angle, cv::Mat& dst) {
cv::Mat r = getRotationMatrix2D(cv::Point2f(), angle, 1.0);
//4 coordinates of the image
std::vector<cv::Point2f> corners(4);
corners[0] = cv::Point2f(0, 0);
corners[1] = cv::Point2f(0, src.rows);
corners[2] = cv::Point2f(src.cols, 0);
corners[3] = cv::Point2f(src.cols, src.rows);
std::vector<cv::Point2f> cornersTransform(4);
cv::transform(corners, cornersTransform, r);
//Copy the 2x3 transformation matrix into a 3x3 transformation matrix
cv::Mat H = cv::Mat::eye(3, 3, CV_64F);
for(int i = 0; i < 2; i++) {
for(int j = 0; j < 3; j++) {
H.at<double>(i, j) = r.at<double>(i, j);
}
}
double offsetX = 0.0, offsetY = 0.0, maxX = 0.0, maxY = 0.0;
//Get max offset outside of the image and max width / height
for(size_t i = 0; i < 4; i++) {
if(cornersTransform[i].x < offsetX) {
offsetX = cornersTransform[i].x;
}
if(cornersTransform[i].y < offsetY) {
offsetY = cornersTransform[i].y;
}
if(cornersTransform[i].x > maxX) {
maxX = cornersTransform[i].x;
}
if(cornersTransform[i].y > maxY) {
maxY = cornersTransform[i].y;
}
}
offsetX = -offsetX;
offsetY = -offsetY;
maxX += offsetX;
maxY += offsetY;
cv::Size size_warp(maxX, maxY);
//Create the transformation matrix to be able to have all the pixels
cv::Mat H2 = cv::Mat::eye(3, 3, CV_64F);
H2.at<double>(0,2) = offsetX;
H2.at<double>(1,2) = offsetY;
warpPerspective(src, dst, H2*H, size_warp);
}
void FFreeFormBoardData::AddMarker(FMarkerDefinitionData const & marker_def)
{
ArucoBoard.ids.push_back(marker_def.MarkerId);
std::vector<cv::Point3f> corners(4);
for (int idx = 0; idx < 4; idx++)
{
corners[idx] = cv::Point3f(FAUROpenCV::ConvertUnrealVectorToOpenCv(marker_def.Corners[idx]));
}
ArucoBoard.objPoints.push_back(corners);
}
CV_IMPL void
cvFindCornerSubPix( const void* srcarr, CvPoint2D32f* _corners,
int count, CvSize win, CvSize zeroZone,
CvTermCriteria criteria )
{
if(!_corners || count <= 0)
return;
cv::Mat src = cv::cvarrToMat(srcarr), corners(count, 1, CV_32FC2, _corners);
cv::cornerSubPix(src, corners, win, zeroZone, criteria);
}
void TopologyInfo::side_from_higher_order( EntityTopology topo,
unsigned num_nodes,
unsigned node_number,
unsigned& side_dim_out,
unsigned& side_num_out,
MsqError& err )
{
bool midedge, midface, midvol;
higher_order( topo, num_nodes, midedge, midface, midvol, err );
MSQ_ERRRTN(err);
side_num_out = node_number;
if (side_num_out < corners(topo)) {
side_dim_out = 0;
return;
}
side_num_out -= corners(topo);
if (midedge) {
if (side_num_out < edges(topo)) {
side_dim_out = 1;
return;
}
side_num_out -= edges(topo);
}
if (midface) {
if (side_num_out < faces(topo)) {
side_dim_out = 2;
return;
}
side_num_out -= faces(topo);
}
if (midvol && side_num_out == 0) {
side_dim_out = 3;
return;
}
MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid node index\n");
}
int HEG_system::enforcePbc(Array1 <doublevar> & pos, Array1 <int> & nshifted) {
assert(pos.GetDim(0) >=3);
int shifted=0;
nshifted.Resize(3);
nshifted=0;
int nshift=0;
for(int i=0; i< 3; i++) {
int shouldcontinue=1;
while(shouldcontinue) {
//Whether we're past the origin side
doublevar tooshort=0;
for(int j=0; j<3;j++) tooshort+=normVec(i,j)*(pos(j)-origin(j));
//Whether we're past the lattice vector
doublevar toofar=0;
for(int j=0; j< 3; j++) toofar+=normVec(i,j)*(pos(j)-corners(i,j));
//the 1e-12 seems to help avoid numerical problems, esp
//when integrating over a grid(which tends to hit the edges)
if(tooshort < -1e-12) {
//cout <<"tooshort " << tooshort << endl;
for(int j=0; j< 3; j++) pos(j)+=latVec(i,j);
shifted=1;
nshifted(i)+=1;
nshift++;
}
else if(toofar > 1e-12) {
//cout << "toofar " << toofar << endl;
for(int j=0; j< 3; j++) pos(j)-=latVec(i,j);
shifted=1;
// JK: here was +1, which works fine for real k-points that
// correspond to standing waves. For general (complex) k-points the
// wavefunction is "directional", however.
nshifted(i)-=1;
nshift++;
}
else {
shouldcontinue=0;
}
if(nshift > 1000)
error("Did over 1000 shifts and we're still out of the simulation cell."
" There's probably something wrong. Position : ", pos(i));
}
}
return shifted;
}
void setupGeometry(int elementIndex, Geometry& geometry) const
{
const int dimGrid = m_vertices.n_rows;
size_t cornerCount = 0;
for (; cornerCount < m_elementCornerIndices.n_rows; ++cornerCount)
if (m_elementCornerIndices(cornerCount, elementIndex) < 0)
break;
arma::Mat<CoordinateType> corners(dimGrid, cornerCount);
for (size_t cornerIndex = 0; cornerIndex < cornerCount; ++cornerIndex)
corners.col(cornerIndex) = m_vertices.col(
m_elementCornerIndices(cornerIndex, elementIndex));
geometry.setup(corners, m_auxData.unsafe_col(elementIndex));
}
vector<Vector> AABB::getCorners() const
{
vector<Vector> corners( 8 ) ;
corners.push_back( Vector( min.x,min.y,min.z ) ) ;
corners.push_back( Vector( min.x,min.y,max.z ) ) ;
corners.push_back( Vector( min.x,max.y,min.z ) ) ;
corners.push_back( Vector( min.x,max.y,max.z ) ) ;
corners.push_back( Vector( max.x,min.y,min.z ) ) ;
corners.push_back( Vector( max.x,min.y,max.z ) ) ;
corners.push_back( Vector( max.x,max.y,min.z ) ) ;
corners.push_back( Vector( max.x,max.y,max.z ) ) ;
return corners ;
}
void FastDetector::detect(
Frame* frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts)
{
Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
for(int L=0; L<n_pyr_levels_; ++L)
{
const int scale = (1<<L);
vector<fast::fast_xy> fast_corners;
#if __SSE2__
fast::fast_corner_detect_10_sse2(
(fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
#elif HAVE_FAST_NEON
fast::fast_corner_detect_9_neon(
(fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
#else
fast::fast_corner_detect_10(
(fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
#endif
vector<int> scores, nm_corners;
fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);
fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);
for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it)
{
fast::fast_xy& xy = fast_corners.at(*it);
const int k = static_cast<int>((xy.y*scale)/cell_size_)*grid_n_cols_
+ static_cast<int>((xy.x*scale)/cell_size_);
if(grid_occupancy_[k])
continue;
const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y);
if(score > corners.at(k).score)
corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
}
}
// Create feature for every corner that has high enough corner score
std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
if(c.score > detection_threshold)
fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
});
resetGrid();
}
const unsigned* TopologyInfo::adjacent_vertices( EntityTopology topo,
unsigned index,
unsigned& num_adj_out )
{
const unsigned count = corners( topo );
if (!count || index >= count)
{
num_adj_out = 0;
return 0;
}
const unsigned* vect = instance.vertAdjMap[topo-FIRST_FACE][index];
num_adj_out = vect[0];
return vect + 1;
}
std::shared_ptr<Carta::Lib::RegionInfo> Region::getInfo() const {
std::shared_ptr<Carta::Lib::RegionInfo> info( new Carta::Lib::RegionInfo() );
info->setRegionType( getRegionType() );
int cornerCount = m_state.getArraySize( CORNERS );
std::vector< std::pair<double,double> > corners( cornerCount );
for( int i = 0; i < cornerCount; i++ ){
QString eleLookup = Carta::State::UtilState::getLookup( CORNERS, i );
QString xLookup = Carta::State::UtilState::getLookup( eleLookup, Util::XCOORD );
QString yLookup = Carta::State::UtilState::getLookup( eleLookup, Util::YCOORD );
double xValue = m_state.getValue<double>( xLookup );
double yValue = m_state.getValue<double>( yLookup );
corners[i] = std::pair<double,double>( xValue, yValue );
}
info->setCorners( corners );
return info;
}
