PointVector Track::getTrackTriangleStripPoints() const
{
PointVector vStripPoints;
if(m_vPathPoints.size() < 2)
{
return vStripPoints;
}
// First 2 points
NMVector2f v = getNormalizedPerpendicularVector(m_vPathPoints[1] - m_vPathPoints[0]) * m_fTrackWidth;
vStripPoints.push_back(m_vPathPoints[0] + v);
vStripPoints.push_back(m_vPathPoints[0] - v);
// All points in the middle
for(NLib::NSize_t i = 1; i < m_vPathPoints.size() - 1; ++i)
{
NMVector2f v1 = m_vPathPoints[i] - m_vPathPoints[i - 1];
NMVector2f v2 = m_vPathPoints[i + 1] - m_vPathPoints[i];
NMVector2f n = getNormalizedPerpendicularVector(v1 + v2) * m_fTrackWidth;
vStripPoints.push_back(m_vPathPoints[i] + n);
vStripPoints.push_back(m_vPathPoints[i] - n);
}
// Last 2 points
NLib::NSize_t uLastIndex = m_vPathPoints.size() - 1;
v = getNormalizedPerpendicularVector(m_vPathPoints[uLastIndex] - m_vPathPoints[uLastIndex - 1]) * m_fTrackWidth;
vStripPoints.push_back(m_vPathPoints[uLastIndex] + v);
vStripPoints.push_back(m_vPathPoints[uLastIndex] - v);
return vStripPoints;
}
void SphericalPointProjector::unProject(PointVector &points,
IntImage &indexImage,
const DepthImage &depthImage) const {
if(_imageRows == 0 || _imageCols == 0) {
throw "SphericalPointProjector: Depth image has zero dimensions";
}
points.resize(depthImage.rows * depthImage.cols);
int count = 0;
indexImage.create(depthImage.rows, depthImage.cols);
Point *point = &points[0];
for(int r = 0; r < depthImage.rows; r++) {
const float *f = &depthImage(r, 0);
int *i = &indexImage(r, 0);
for(int c = 0; c < depthImage.cols; c++, f++, i++) {
if(!_unProject(*point, c, r, *f)) {
*i = -1;
continue;
}
point++;
*i = count;
count++;
}
}
points.resize(count);
}
PointPairVector Space::pairsInZone(const PointVector& zone, unsigned int d)
{
PointPairVector result;
PointVector::const_iterator iter = zone.begin();
PointVector::const_iterator endIter = zone.end();
for(;iter!=endIter;++iter)
{
for(unsigned int i = 1;(iter+i)!=endIter;++i)
{
// optimization
if(iter->distanceYTo(*(iter+i))>d) // if distance by Y is higher than d
{
if(iter->y < (iter+i)->y) // if current point is before second one
break; // there won't be more interesting points in d-neighborhood
else
continue; // we could not reach interesting points yet
}
if(iter->distanceTo(*(iter+i))<=d) // if distance is lower than d
{
result.insert(PointPair(*iter,*(iter+i))); // add pair of d-neighbors
}
}
}
return result;
}
void CylindricalPointProjector::unProject(PointVector &points,
Gaussian3fVector &gaussians,
IntImage &indexImage,
const DepthImage &depthImage) const {
assert(depthImage.rows > 0 && depthImage.cols > 0 && "CylindricalPointProjector: Depth image has zero dimensions");
points.resize(depthImage.rows * depthImage.cols);
gaussians.resize(depthImage.rows * depthImage.cols);
indexImage.create(depthImage.rows, depthImage.cols);
int count = 0;
Point *point = &points[0];
Gaussian3f *gaussian = &gaussians[0];
for(int r = 0; r < depthImage.rows; r++) {
const float *f = &depthImage(r, 0);
int *i = &indexImage(r, 0);
for(int c = 0; c < depthImage.cols; c++, f++, i++) {
if(!_unProject(*point, c, r, *f)) {
*i = -1;
continue;
}
Eigen::Matrix3f cov = Eigen::Matrix3f::Identity();
*gaussian = Gaussian3f(point->head<3>(), cov);
gaussian++;
point++;
*i = count;
count++;
}
}
points.resize(count);
gaussians.resize(count);
}
bool testIntegerNorms()
{
unsigned int nbok = 0;
unsigned int nb = 0;
DGtal::int32_t t[]= {2,1,3,4};
PointVector<4,DGtal::int32_t> p1(t);
DGtal::int32_t t2[]= {4,5,3,2};
PointVector<4,DGtal::int32_t> p2(t2);
PointVector<4,DGtal::int32_t> p = p2 - p1;
trace.beginBlock ( "Checking Integer norm1" );
trace.info() << "p1: "<<p1 <<", "<<"p2: "<<p2 <<std::endl;
nbok += p.norm1() == 8 ? 1 : 0;
nb++;
trace.info() << "(" << nbok << "/" << nb << ") "
<< "L1(p2-p1): "<< p.norm1() << "( == 8 ?)" << std::endl;
nbok += p.normInfinity() == 4 ? 1 : 0;
nb++;
trace.info() << "(" << nbok << "/" << nb << ") "
<< "Linfty(p2-p1): "<< p.normInfinity() << "( == 4 ?)"
<< std::endl;
trace.endBlock();
return nbok == nb;
}
void MouseMovementManager::mouseMove(QPointF const & pnt)
{
PointVector path = mPath.back();
mPath.pop_back();
path.push_back(pnt);
mPath.push_back(path);
recountCentre();
}
void SparseSurfaceAdjustmentT<EdgeType1, EdgeType2, EdgeType3>::optimizeColorsIncidenteWeight()
{
for(int i = 0; i < (int) graph()->_verticies_points.size(); ++i){
PointVertex* point = graph()->_verticies_points[i];
std::vector<PointVertex* > neighborhood;
double totalWeight = 0;
double r = 0; //point->cr;
double g = 0; //point->cg;
double b = 0; //point->cb;
//find neighborhood
for (g2o::OptimizableGraph::EdgeSet::iterator it=point->edges().begin(); it!=point->edges().end(); it++){
EdgeType3* edge=dynamic_cast<EdgeType3*>(*it);
if(edge){
PointVertex* vp1=dynamic_cast<PointVertex*>(edge->vertices()[0]);
PointVertex* vp2=dynamic_cast<PointVertex*>(edge->vertices()[1]);
if(point->id() != vp1->id()){
neighborhood.push_back(vp1);
}
if(point->id() != vp2->id()){
neighborhood.push_back(vp2);
}
}
}
///Add weighted color values
for(size_t j = 0; j < neighborhood.size(); ++j){
PointVertex*& n = neighborhood[j];
///find sensor edge and calculate incidence angle
PoseVertex* nParent = n->parentVertex();
PointVector beam = nParent->estimate().translation() - n->estimate();
double incidenceAngle = fabs(acos(beam.dot(n->normal())));
double weight = 1.0; // - (incidenceAngle / (M_PI * 0.5));
// weight = min(weight, 0.0);
// weight *= weight;
// if(RAD2DEG(incidenceAngle) > 60.0)
// weight = 0.0;
r += n->cr * weight;
g += n->cg * weight;
b += n->cb * weight;
totalWeight += weight;
}
//set colors
if(neighborhood.size() > 1){
point->cr = r / totalWeight;
point->cg = g / totalWeight;
point->cb = b / totalWeight;
for(size_t j = 0; j < neighborhood.size(); ++j){
PointVertex*& n = neighborhood[j];
n->cr = point->cr;
n->cg = point->cg;
n->cb = point->cb;
}
}
}
}
double PathDistance(PointVector& pts1, PointVector& pts2)
{
double d = 0.0;
for (size_t i = 0; i < pts1.size(); ++i) // assumes pts1.length == pts2.length
{
d += Distance(pts1[i], pts2[i]);
}
return d / pts1.size();
}
void util::printPointVector(const PointVector pVect, int fieldWidth)
{
PointVector::const_iterator point;
for (point = pVect.begin(); point != pVect.end(); ++point) {
std::cout << "["
<< std::setw(fieldWidth) << (*point).x << " , "
<< std::setw(fieldWidth) << (*point).y << "] ";
}
std::cout << std::endl;
}
PointVector Space::leaveFromZone(const PointVector& vec, const Point& splitPoint, unsigned int d)
{
PointVector result;
PointVector::const_iterator iter = vec.begin();
PointVector::const_iterator endIter = vec.end();
for(;iter!=endIter;++iter)
{
if(iter->distanceXTo(splitPoint)<=d)
result.push_back(*iter);
}
return result;
}
PointPtr Centroid(PointVector& points)
{
double x = 0;
double y = 0;
for (size_t i = 0; i < points.size(); ++i)
{
x += points[i]->X;
y += points[i]->Y;
}
x /= points.size();
y /= points.size();
return PointPtr(new Point(x, y));
}
void fourier_broken(T &m, feature_t* buf) {
int dftCount = FDLENGTH;
typename ImageFactory<T>::view_type *tmp = simple_image_copy(m);
// get contour points for each CC
ImageList* ccs = cc_analysis(*tmp);
PointVector p;
for(ImageList::iterator cc_it = ccs->begin(); cc_it != ccs->end();
cc_it++) {
Cc* cc = static_cast<Cc*>(*cc_it);
Point orig = cc->origin();
PointVector* cc_p = contour_pavlidis(*cc);
for(PointVector::iterator p_it = cc_p->begin();
p_it != cc_p->end(); p_it++) {
p.push_back(*p_it + orig);
}
delete *cc_it;
delete cc_p;
}
delete ccs;
delete tmp->data();
delete tmp;
if (p.size() == 0) {
for (int k = 0; k < dftCount; k++)
buf[k] = 0.0;
return;
}
else if (p.size() == 1) {
buf[0] = 1.0;
for (int k = 1; k < dftCount; k++)
buf[k] = 0.0;
return;
}
// calculate convex hull and interpolate points
PointVector* hullPoints = convex_hull_from_points(&p);
FloatPointVector* interpolatedHullPoints =
interpolatePolygonPoints(hullPoints);
FloatVector* distances = minimumContourHullDistances(interpolatedHullPoints, &p);
floatFourierDescriptorBrokenA(interpolatedHullPoints, &p, distances, dftCount, buf);
delete hullPoints;
delete interpolatedHullPoints;
delete distances;
}
void PeakDetect::findAll(float delta) {
clear();
Point max(-1, -1);
Point min(-1, 65536);
bool findMax = false;
for (index = 0; index < length; index++) {
// check if max/min should be updated
if (y[index] > max.getY()) max = Point(index, y[index]);
if (minima->size()) {
if (y[index] < min.getY()) min = Point(index, y[index]);
} else {
if (y[index] <= min.getY()) min = Point(index, y[index]);
}
// check if max/min are larger than delta
if (findMax) {
if (y[index] < max.getY() - delta) {
maxima->add(max.getX(), max.getY());
min = Point(index, y[index]);
findMax = false;
}
} else {
if (y[index] > min.getY() + delta) {
minima->add(min.getX(), min.getY());
max = Point(index, y[index]);
findMax = true;
}
}
}
// add last minimum
minima->add(min.getX(), min.getY());
if (x != NULL) {
PointVector *maxtemp = new PointVector();
PointVector *mintemp = new PointVector();
for (int i = 0; i < maxima->size(); i++)
maxtemp->add(x[(maxima->get(i))->getX()], (maxima->get(i))->getY());
for (int i = 0; i < minima->size(); i++)
mintemp->add(x[(minima->get(i))->getX()], (minima->get(i))->getY());
delete maxima;
delete minima;
maxima = maxtemp;
minima = mintemp;
}
}
PointVector rectangle(const QRectF &rect)
{
const QPointF p1 = rect.topLeft();
const QPointF p2 = rect.bottomRight();
PointVector pv;
pv.reserve(5);
pv << Point(p1, 1);
pv << Point(p1.x(), p2.y(), 1);
pv << Point(p2, 1);
pv << Point(p2.x(), p1.y(), 1);
pv << Point(p1.x() + 1, p1.y(), 1);
return pv;
}
PointVector ImageRegistrator::removeBadPoints(PointVector points, std::vector<uchar> status)
{
std::vector<uchar>::iterator statusIter = status.begin();
PointVector::iterator pointIter = points.begin();
PointVector goodPoints(points.size() - std::count(status.begin(), status.end(), 0));
for (int i = 0; statusIter != status.end(); ++statusIter, ++pointIter) {
if (*statusIter == 1) {
goodPoints[i] = *pointIter;
++i;
}
}
return goodPoints;
}
void PinholePointProjector::project(IntImage &indexImage,
DepthImage &depthImage,
const PointVector &points) const {
assert(_imageRows && _imageCols && "PinholePointProjector: _imageRows and _imageCols are zero");
indexImage.create(_imageRows, _imageCols);
depthImage.create(_imageRows, _imageCols);
depthImage.setTo(cv::Scalar(std::numeric_limits<float>::max()));
indexImage.setTo(cv::Scalar(-1));
float *drowPtrs[_imageRows];
int *irowPtrs[_imageRows];
for(int i = 0; i < _imageRows; i++) {
drowPtrs[i] = &depthImage(i, 0);
irowPtrs[i] = &indexImage(i, 0);
}
const Point *point = &points[0];
for(size_t i = 0; i < points.size(); i++, point++) {
int x, y;
float d;
if(!_project(x, y, d, *point) ||
d < _minDistance || d > _maxDistance ||
x < 0 || x >= indexImage.cols ||
y < 0 || y >= indexImage.rows)
continue;
float &otherDistance = drowPtrs[y][x];
int &otherIndex = irowPtrs[y][x];
if(!otherDistance || otherDistance > d) {
otherDistance = d;
otherIndex = i;
}
}
}
int FilteredProcess::eliminate_triangle(const Triangle &triangle)
{
if (points_.size() != 2 * get_sensor_cnt())
return -1;
PointVector valid;
for (auto p : points_)
if (!triangle.is_inside(p))
valid.push_back(p);
if (valid.size() != get_sensor_cnt())
return -1;
points_ = valid;
return 0;
}
void writeVtkVectorArray(ostream &out, const string &name, const PointVector &data) {
out << " <DataArray type=\"Float64\" Name=\"" << name << "\" NumberOfComponents=\"3\" format=\"ascii\">";
for (int c = 0; c < data.size(); c++) {
Vector3d point = data[c];
out << " " << point[0] << " " << point[1] << " " << point[2];
}
out << " </DataArray>" << endl;
}
void Whiteout::DoWhiteout(
Space space,
Rectangle rect,
Point starting,
Mask mask,
bool clockwise, bool viral)
{
unsigned int depth;
PointVector pv;
// Loop around the outside of the rectangle. Call whiteout helper at each
// pixel.
for (OutsideRectangleIterator it(space, rect, starting); it.More();)
{
depth = 0;
if (!WhiteoutHelper(viral, mask, depth, pv, it.GetCurrent()) &&
!viral)
{
return;
}
while (!pv.empty())
{
Point vpoint = pv.front();
pv.pop_front();
depth = 0;
if (!WhiteoutHelper(viral, mask, depth, pv, vpoint) &&
!viral)
{
return;
}
}
// Move either clockwise or counter clockwise.
if (clockwise)
{
it++;
}
else
{
it--;
}
}
}
PointPairVector Space::bruteNeighbors(const PointVector& vec, unsigned int start, unsigned int end, unsigned int d) const
{
PointPairVector result;
PointVector::const_iterator i = vec.begin()+start;
PointVector::const_iterator j = vec.begin()+start+1;
PointVector::const_iterator endIter = (end>=vec.size()) ? vec.end() : vec.begin()+end+1;
for(;i<endIter;++i)
{
for(j=i+1;j<endIter;++j)
{
if(j->distanceTo(*i)<=d)
{
result.insert(PointPair(*i,*j));
}
}
}
return result;
}
double PathLength(PointVector& points)
{
double d = 0.0;
for (size_t i = 1; i < points.size(); ++i)
{
d += Distance(points[i - 1], points[i]);
}
return d;
}
void auxdata::DrcPoly::motionDraw(const layprop::DrawProperties&, CtmQueue& transtack,
SGBitSet* plst) const
{
CTM trans = transtack.front();
PointVector* ptlist = DEBUG_NEW PointVector;
ptlist->reserve(_psize);
for (unsigned i = 0; i < _psize; i++)
{
ptlist->push_back( TP(_pdata[2*i], _pdata[2*i+1]) * trans);
}
glBegin(GL_LINE_LOOP);
for (unsigned i = 0; i < _psize; i++)
{
glVertex2i((*ptlist)[i].x(), (*ptlist)[i].y());
}
glEnd();
ptlist->clear();
delete ptlist;
}
/*!
* \brief buildMassCenters Use this function to retrieve the mass centers of _contours.
* \param _contours Input contours.
* \param _massCenters Output mass centers.
*/
void ContourManager::buildMassCenters(const ContourVector & _contours, PointVector & _massCenters)
{
ContourVector::size_type contourSize = _contours.size();
_massCenters.resize(contourSize);
for (ContourVector::size_type i = 0; i < contourSize; ++i)
{
massCenter(_contours[i],_massCenters[i]);
}//for(ContourVector::size_type i = 0; i < contourSize; ++i)
}//buildMassCenters
TerrainCoverDrawableObject::NormaleMap
TerrainCoverDrawableObject::GetNormales(const PointVector & pv, const IndexVector & iv)
{
NormaleMap normales;
auto normaleCalculator = [&](int a, int b, int c)
{
const Point3d normale = utils::GetNormaleForTriangleNonNormalized(pv.at(a), pv.at(b), pv.at(c));
normales[a] += normale;
normales[b] += normale;
normales[c] += normale;
};
utils::ContainerAggregator<decltype(normaleCalculator), int, 3> aggregator(normaleCalculator);
boost::for_each(iv, aggregator);
return normales;
}
void StatsCalculator::compute(NormalVector &normals,
StatsVector &statsVector,
const PointVector &points,
const IntImage &indexImage) {
assert(indexImage.rows > 0 && indexImage.cols > 0 && "StatsCalculator: indexImage has zero size");
assert(points.size() > 0 && "StatsCalculator: points has zero size");
// HAKKE in order to avoid an annoying warning
if(indexImage.rows > 0 && indexImage.cols > 0) {
assert(1);
}
if(statsVector.size() != points.size())
statsVector.resize(points.size());
if(normals.size() != points.size())
normals.resize(points.size());
Normal dummyNormal = Normal::Zero();
std::fill(statsVector.begin(), statsVector.end(), Stats());
std::fill(normals.begin(), normals.end(), dummyNormal);
}
PointVector sampleStroke(const QRectF &rect)
{
const int strokew = rect.width();
const qreal strokeh = rect.height() * 0.6;
const qreal offy = rect.top() + rect.height()/2;
const qreal dphase = (2*M_PI)/qreal(strokew);
qreal phase = 0;
PointVector pointvector;
pointvector.reserve(strokew);
pointvector << paintcore::Point(rect.left(), offy, 0.0);
for(int x=0;x<strokew;++x, phase += dphase) {
const qreal fx = x/qreal(strokew);
const qreal pressure = qBound(0.0, ((fx*fx) - (fx*fx*fx))*6.756, 1.0);
const qreal y = qSin(phase) * strokeh;
pointvector << Point(rect.left()+x, offy+y, pressure);
}
return pointvector;
}
void TranslateToOrigin(PointVector& points)
{
PointPtr c = Centroid(points);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = points[i]->X - c->X;
double qy = points[i]->Y - c->Y;
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
void ScaleToSquare(PointVector& points, double size)
{
RectanglePtr B = BoundingBox(points);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = points[i]->X * (size / B->Width);
double qy = points[i]->Y * (size / B->Height);
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
void PinholePointProjector::unProject(PointVector &points,
Gaussian3fVector &gaussians,
IntImage &indexImage,
const DepthImage &depthImage) const {
assert(depthImage.rows > 0 && depthImage.cols > 0 && "PinholePointProjector: Depth image has zero dimensions");
points.resize(depthImage.rows * depthImage.cols);
gaussians.resize(depthImage.rows * depthImage.cols);
indexImage.create(depthImage.rows, depthImage.cols);
int count = 0;
Point *point = &points[0];
Gaussian3f *gaussian = &gaussians[0];
float fB = _baseline * _cameraMatrix(0, 0);
Eigen::Matrix3f J;
for(int r = 0; r < depthImage.rows; r++) {
const float *f = &depthImage(r, 0);
int *i = &indexImage(r, 0);
for(int c = 0; c < depthImage.cols; c++, f++, i++) {
if(!_unProject(*point, c, r, *f)) {
*i = -1;
continue;
}
float z = *f;
float zVariation = (_alpha * z * z) / (fB + z * _alpha);
J <<
z, 0, (float)r,
0, z, (float)c,
0, 0, 1;
J = _iK * J;
Diagonal3f imageCovariance(3.0f, 3.0f, zVariation);
Eigen::Matrix3f cov = J * imageCovariance * J.transpose();
*gaussian = Gaussian3f(point->head<3>(), cov);
gaussian++;
point++;
*i = count;
count++;
}
}
points.resize(count);
gaussians.resize(count);
}
Template::Template(std::string name, PointVector points) : Name(name), references(0)
{
for (size_t i = 0; i < points.size(); ++i)
{
Points.push_back(PointPtr(new Point(points[i]->X,points[i]->Y)));
}
Resample(Points, NumPoints);
RotateToZero(Points);
ScaleToSquare(Points, SquareSize);
TranslateToOrigin(Points);
}
