void Resample(PointVector& points, size_t n)
{
double I = PathLength(points) / (n - 1); // interval length
double D = 0.0;
PointVector newpoints;
newpoints.push_back(points[0]);
for (size_t i = 1; i < points.size(); ++i)
{
double d = Distance(points[i - 1], points[i]);
if ((D + d) >= I)
{
double qx = points[i - 1]->X + ((I - D) / d) * (points[i]->X - points[i - 1]->X);
double qy = points[i - 1]->Y + ((I - D) / d) * (points[i]->Y - points[i - 1]->Y);
PointPtr q(new Point(qx, qy));
newpoints.push_back(q);
points.insert(points.begin()+i,q);
D = 0.0;
}
else D += d;
}
// somtimes we fall a rounding-error short of adding the last point, so add it if so
if (newpoints.size() == n - 1)
{
newpoints.push_back(points[points.size() - 1]);
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
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();
}
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 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;
}
}
}
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;
}
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 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);
}
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);
}
int Process::initialize(const PointVector &sensors)
{
if (sensors.size() < 3) {
fprintf(stderr, "Process needs at least 3 sensors.\n");
return -1;
}
sensors_ = PointVector(sensors);
clear();
initialized_ = true;
return 0;
}
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());
}
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;
}
RectanglePtr BoundingBox(PointVector& points)
{
int minX = INT_MAX, maxX = INT_MIN, minY = INT_MAX, maxY = INT_MIN;
for (size_t i = 0; i < points.size(); ++i)
{
if (points[i]->X < minX)
minX = points[i]->X;
if (points[i]->X > maxX)
maxX = points[i]->X;
if (points[i]->Y < minY)
minY = points[i]->Y;
if (points[i]->Y > maxY)
maxY = points[i]->Y;
}
return RectanglePtr(new Rectangle(minX, minY, maxX - minX, maxY - minY));
}
void RotateBy(PointVector& points, double theta)
{
PointPtr c = Centroid(points);
double cosine = cos(theta);
double sine = sin(theta);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = (points[i]->X - c->X) * cosine - (points[i]->Y - c->Y) * sine + c->X;
double qy = (points[i]->X - c->X) * sine + (points[i]->Y - c->Y) * cosine + c->Y;
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
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;
}
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;
}
/**
* Returns if there are at least two points for trail rendering
*/
bool HasPoints() const {
return points.size() >= 2;
}
