unsigned int NormalDetector::computeInterestPoints(const LaserReading& reading, const std::vector<double>& signal, std::vector<InterestPoint*>& point,
std::vector< std::vector<unsigned int> >& indexes, std::vector<unsigned int>& maxRangeMapping) const
{
point.clear();
point.reserve(reading.getRho().size());
const std::vector<Point2D>& worldPoints = reading.getWorldCartesian();
for(unsigned int i = 0; i < indexes.size(); i++){
for(unsigned int j = 0; j < indexes[i].size(); j++){
OrientedPoint2D pose;
unsigned int pointIndex = maxRangeMapping[indexes[i][j]];
// Reomoving the detection in the background and pushing it to the foreground
double rangeBefore = (pointIndex > 1)? reading.getRho()[pointIndex - 1] : reading.getMaxRange();
double rangeAfter = (pointIndex < worldPoints.size() - 1)? reading.getRho()[pointIndex + 1] : reading.getMaxRange();
double rangeCurrent = reading.getRho()[pointIndex];
if(rangeBefore < rangeAfter){
if(rangeBefore < rangeCurrent){
pointIndex = pointIndex - 1;
}
} else if(rangeAfter < rangeCurrent){
pointIndex = pointIndex + 1;
}
// Removing max range reading
if(reading.getRho()[pointIndex] >= reading.getMaxRange()){
continue;
}
pose.x = (reading.getWorldCartesian()[pointIndex]).x;
pose.y = (reading.getWorldCartesian()[pointIndex]).y;
pose.theta = normAngle(signal[indexes[i][j]], -M_PI);
bool exists = false;
for(unsigned int k = 0; !exists && k < point.size(); k++){
exists = exists || (fabs(pose.x - point[k]->getPosition().x) <= 0.2 && fabs(pose.y - point[k]->getPosition().y) <= 0.2);
}
if(exists) continue;
unsigned int first = indexes[i][j] - floor((int)m_filterBank[i].size()/2.0);
unsigned int last = indexes[i][j] + floor((int)m_filterBank[i].size()/2.0);
std::vector<Point2D> support(last - first + 1);
for(unsigned int p = 0; p < support.size(); p++) {
support[p] = Point2D(worldPoints[maxRangeMapping[p + first]]);
}
double maxDistance = -1e20;
for(unsigned int k = 0; k < support.size(); k++){
double distance = sqrt((pose.x - support[k].x)*(pose.x - support[k].x) + (pose.y - support[k].y)*(pose.y - support[k].y));
maxDistance = maxDistance < distance ? distance : maxDistance;
}
InterestPoint *interest = new InterestPoint(pose, maxDistance);
// InterestPoint *interest = new InterestPoint(pose, m_scales[i]);
interest->setSupport(support);
interest->setScaleLevel(i);
point.push_back(interest);
}
}
return point.size();
}
void match(unsigned int position)
{
// cairo_set_line_cap(cairoOut, CAIRO_LINE_CAP_ROUND);
m_sensorReference.seek(position);
cairo_matrix_t m1;
cairo_get_matrix(cairoOut, &m1);
cairo_identity_matrix(cairoOut);
cairo_set_source_surface(cairoOut, cairo_get_target(cairoMap), 0., 0.);
cairo_paint(cairoOut);
cairo_set_matrix(cairoOut, &m1);
// cairo_set_line_width(cairoOut, 1./(2.*scaleFactor));
std::vector<InterestPoint *> pointsLocal(m_pointsReference[position].size());
const LaserReading* lreadReference = dynamic_cast<const LaserReading*>(m_sensorReference.current());
for(unsigned int j = 0; j < m_pointsReference[position].size(); j++){
InterestPoint * point = new InterestPoint(*m_pointsReference[position][j]);
point->setPosition(lreadReference->getLaserPose().ominus(point->getPosition()));
pointsLocal[j] = point;
}
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
if(i == position) {
continue;
}
OrientedPoint2D transform;
std::vector< std::pair<InterestPoint*, InterestPoint* > > correspondences;
double result = m_ransac->matchSets(m_pointsReference[i], pointsLocal, transform, correspondences);
if(correspondences.size() >= corresp) {
cairo_matrix_t m;
cairo_get_matrix(cairoOut, &m);
cairo_translate(cairoOut, transform.x, transform.y);
cairo_rotate(cairoOut, transform.theta);
cairo_set_source_rgba(cairoOut, 1., 0., 0., 1. - result/(acceptanceSigma * acceptanceSigma * 5.99 * double(pointsLocal.size())));
cairo_move_to(cairoOut, 0., -0.3);
cairo_line_to(cairoOut, 0.6, 0.);
cairo_line_to(cairoOut, 0., 0.3);
cairo_close_path(cairoOut);
cairo_fill(cairoOut);
cairo_set_matrix(cairoOut, &m);
}
}
cairo_matrix_t m;
cairo_get_matrix(cairoOut, &m);
cairo_translate(cairoOut, lreadReference->getLaserPose().x, lreadReference->getLaserPose().y);
cairo_rotate(cairoOut, lreadReference->getLaserPose().theta);
cairo_set_source_rgba(cairoOut, 0., 0., 1., 1.);
cairo_move_to(cairoOut, 0., -0.3);
cairo_line_to(cairoOut, 0.6, 0.);
cairo_line_to(cairoOut, 0., 0.3);
cairo_close_path(cairoOut);
cairo_stroke(cairoOut);
cairo_set_matrix(cairoOut, &m);
// cairo_show_page(cairoOut);
}
InterestPoint::InterestPoint(const InterestPoint& _point):
m_position(_point.getPosition()),
m_scale(_point.getScale()),
m_scaleLevel(_point.getScaleLevel())
{
if(_point.getDescriptor())
m_descriptor = _point.getDescriptor()->clone();
else
m_descriptor = 0;
}
InterestPoint* fromRos (const InterestPointRos& m)
{
OrientedPoint2D pose(m.pose.x, m.pose.y, m.pose.theta);
Descriptor* descriptor = fromRos(m.descriptor);
InterestPoint* pt = new InterestPoint(pose, m.scale, descriptor);
pt->setScaleLevel(m.scale_level);
vector<Point2D> support_points(m.support_points.size());
transform(m.support_points.begin(), m.support_points.end(),
support_points.begin(), toPoint2D);
pt->setSupport(support_points);
return pt;
}
void writeBoW(std::ofstream& out){
std::string bar(50, ' ');
bar[0] = '#';
unsigned int progress = 0;
struct timeval start, end;
gettimeofday(&start, NULL);
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
unsigned int currentProgress = (i*100)/(m_pointsReference.size() - 1);
if (progress < currentProgress){
progress = currentProgress;
bar[progress/2] = '#';
std::cout << "\rDescribing scans [" << bar << "] " << progress << "%" << std::flush;
}
std::multimap<double,WordResult> signature;
for(unsigned int j = 0; j < m_pointsReference[i].size(); j++){
InterestPoint * point = m_pointsReference[i][j];
OrientedPoint2D localpose = m_posesReference[i].ominus(point->getPosition());
double angle = atan2(localpose.y, localpose.x);
unsigned int bestWord = 0;
double bestMatch = 0.;
std::vector<double> descriptor;
std::vector<double> weights;
point->getDescriptor()->getWeightedFlatDescription(descriptor, weights);
HistogramFeatureWord word(descriptor, NULL, weights);
for(unsigned int w = 0; w < histogramVocabulary.size(); w++) {
double score = histogramVocabulary[w].sim(&word);
if(score > bestMatch) {
bestMatch = score;
bestWord = w;
}
}
WordResult best; best.pose = localpose; best.word = bestWord;
signature.insert(std::make_pair(angle,best));
}
out << m_pointsReference[i].size();
for(std::multimap<double,WordResult>::const_iterator it = signature.begin(); it != signature.end(); it++){
out << " " << it->second.word << " " << it->second.pose.x << " " << it->second.pose.y;
}
out <<std::endl;
}
gettimeofday(&end,NULL);
timersub(&end,&start,&vocabularyTime);
std::cout << " done." << std::endl;
}
InterestPointRos toRos (const InterestPoint& pt)
{
InterestPointRos m;
m.pose.x = pt.getPosition().x;
m.pose.y = pt.getPosition().y;
m.pose.theta = pt.getPosition().theta;
m.support_points.reserve(pt.getSupport().size());
BOOST_FOREACH (const Point2D& p, pt.getSupport())
m.support_points.push_back(toPoint(p));
m.scale = pt.getScale();
m.scale_level = pt.getScaleLevel();
m.descriptor = toRos(pt.getDescriptor());
return m;
}
unsigned int CurvatureDetector::computeInterestPoints(const LaserReading& reading, const std::vector< std::vector<Point2D> >& operatorA, std::vector<InterestPoint*>& point,
const std::vector< std::vector<unsigned int> >& indexes, std::vector<unsigned int>& maxRangeMapping) const
{
point.clear();
point.reserve(reading.getRho().size());
const std::vector<Point2D>& worldPoints = reading.getWorldCartesian();
for(unsigned int i = 0; i < indexes.size(); i++){
for(unsigned int j = 0; j < indexes[i].size(); j++){
OrientedPoint2D pose;
unsigned int pointIndex = maxRangeMapping[indexes[i][j]];
// Reomoving the detection in the background and pushing it to the foreground
double rangeBefore = (pointIndex > 1)? reading.getRho()[pointIndex - 1] : reading.getMaxRange();
double rangeAfter = (pointIndex < worldPoints.size() - 1)? reading.getRho()[pointIndex + 1] : reading.getMaxRange();
double rangeCurrent = reading.getRho()[pointIndex];
if(rangeBefore < rangeAfter){
if(rangeBefore < rangeCurrent){
pointIndex = pointIndex - 1;
}
} else if(rangeAfter < rangeCurrent){
pointIndex = pointIndex + 1;
}
// Removing max range reading
if(reading.getRho()[pointIndex] >= reading.getMaxRange()){
continue;
}
pose.x = (worldPoints[pointIndex]).x;
pose.y = (worldPoints[pointIndex]).y;
Point2D difference = operatorA[i][indexes[i][j]] - worldPoints[pointIndex];
pose.theta = atan2(difference.y, difference.x);
bool exists = false;
for(unsigned int k = 0; !exists && k < point.size(); k++){
exists = exists || (fabs(pose.x - point[k]->getPosition().x) <= 0.2 && fabs(pose.y - point[k]->getPosition().y) <= 0.2);
}
if(exists) continue;
double distance = 2. * m_scales[i];
Point2D diffStart = pose - worldPoints.front();
Point2D diffEnd = pose - worldPoints.back();
if(hypot(diffStart.x, diffStart.y) < distance || hypot(diffEnd.x, diffEnd.y) < distance){
continue;
}
std::vector<Point2D> support;
for(unsigned int k = 0; k < worldPoints.size(); k++){
Point2D diff2 = pose - worldPoints[k];
if(hypot(diff2.x, diff2.y) < distance) support.push_back(worldPoints[k]);
}
LineParameters param = computeNormals(support);
pose.theta = normAngle(param.alpha, - M_PI);
InterestPoint *interest = new InterestPoint(pose, distance);
// InterestPoint *interest = new InterestPoint(pose, m_scales[i]);
interest->setSupport(support);
interest->setScaleLevel(i);
point.push_back(interest);
}
}
return point.size();
}
vector<InterestPoint> InterestPointsDetector::calculateOrientations(
const vector<InterestPoint>& points) {
Kernel gaussKernel = Kernel::createGaussKernelByRadius(radiusOfNeighborhood);
vector<InterestPoint> result = vector<InterestPoint>();
foreach (InterestPoint point, points) {
float rBaskets[ROTATION_INV_BASKET_COUNT] = {0.f};
//заполнить корзины
for (int dx = -radiusOfNeighborhood; dx <= radiusOfNeighborhood; ++dx) {
for (int dy = -radiusOfNeighborhood; dy <= radiusOfNeighborhood; ++dy) {
int x1 = ImageUtil::handleEdgeEffect(dx + point.x + 1, source.getWidth(), type);
int x2 = ImageUtil::handleEdgeEffect(dx + point.x - 1, source.getWidth(), type);
int y1 = ImageUtil::handleEdgeEffect(dy + point.y + 1, source.getHeight(), type);
int y2 = ImageUtil::handleEdgeEffect(dy + point.y - 1, source.getHeight(), type);
int x0 = ImageUtil::handleEdgeEffect(dx + point.x, source.getWidth(), type);
int y0 = ImageUtil::handleEdgeEffect(dy + point.y, source.getHeight(), type);
if(ImageUtil::insideImage(x1, y0)
&& ImageUtil::insideImage(x2, y0)
&& ImageUtil::insideImage(x0, y1)
&& ImageUtil::insideImage(x0, y2)){
float lx1y0 = source.getValue(x1, y0);
float lx2y0 = source.getValue(x2, y0);
float lx0y1 = source.getValue(x0, y1);
float lx0y2 = source.getValue(x0, y2);
float dlx = lx1y0 - lx2y0;
float dly = lx0y1 - lx0y2;
float gradientLength = sqrtf(dlx * dlx + dly * dly);
float angle = atan2(dly, dlx) + M_PI;
gradientLength *= gaussKernel.getValue(dx, dy);
//узнать номер текущей козины
int basketNumber = (int)(angle / R_DPHI);
float mod = angle - basketNumber * R_DPHI;
//узнать номер соседней корзины
int basketNumberNei = calculateNumberOfNeighbornBasket(
R_HALF_DPHI, mod,
basketNumber, ROTATION_INV_BASKET_COUNT);
//узнать расстояние до центра текущей корзины
float centerOfBasket = basketNumber * R_DPHI + R_HALF_DPHI;
float distanceToCenter = fabs(angle - fabs(centerOfBasket));
float distanceToCenterNei = R_DPHI - distanceToCenter;
float ratio = 1. - distanceToCenter / R_DPHI;
float ratioNei = 1. - distanceToCenterNei / R_DPHI;
rBaskets[basketNumber] += gradientLength * ratio;
rBaskets[basketNumberNei] += gradientLength * ratioNei;
}
}
}
//найти пики
int peakIndex = 0;
float peakValue = rBaskets[peakIndex];
for (int i = 1; i < ROTATION_INV_BASKET_COUNT; ++i) {
if (rBaskets[i] > peakValue) {
peakValue = rBaskets[i];
peakIndex = i;
}
}
int xL, xR;
if (peakIndex == 0){
xL = ROTATION_INV_BASKET_COUNT - 1;
} else {
xL = peakIndex - 1;
}
if (peakIndex == ROTATION_INV_BASKET_COUNT - 1){
xR = 0;
} else {
xR = peakIndex + 1;
}
double angleOfPeak = maxInterp3f(rBaskets[xL], peakValue, rBaskets[xR]);
point.setOrientation(angleOfPeak);
result.push_back(point);
int subPeakIndex = -1;
float subPeakValue = 0.8 * peakValue;
for (int i = 0; i < ROTATION_INV_BASKET_COUNT; ++i) {
if (peakIndex != i && rBaskets[i] > subPeakValue) {
subPeakValue = rBaskets[i];
subPeakIndex = i;
}
}
if (subPeakIndex != -1) {
if (subPeakIndex == 0){
xL = ROTATION_INV_BASKET_COUNT - 1;
} else {
xL = subPeakIndex - 1;
}
if (subPeakIndex == ROTATION_INV_BASKET_COUNT - 1){
xR = 0;
} else {
xR = subPeakIndex + 1;
}
double angleOfSubPeak = maxInterp3f(
rBaskets[xL], subPeakValue, rBaskets[xR]);
InterestPoint copy = InterestPoint(point.x, point.y, point.value);
copy.setOrientation(angleOfSubPeak);
result.push_back(copy);
}
}
void match(unsigned int position)
{
m_sensorReference.seek(position);
std::vector<InterestPoint *> pointsLocal(m_pointsReference[position].size());
const LaserReading* lreadReference = dynamic_cast<const LaserReading*>(m_sensorReference.current());
for(unsigned int j = 0; j < m_pointsReference[position].size(); j++){
InterestPoint * point = new InterestPoint(*m_pointsReference[position][j]);
point->setPosition(lreadReference->getLaserPose().ominus(point->getPosition()));
pointsLocal[j] = point;
}
unsigned int inliers[m_pointsReference.size()];
double results[m_pointsReference.size()];
double linearErrors[m_pointsReference.size()];
double angularErrors[m_pointsReference.size()];
struct timeval start, end, diff, sum;
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
if(fabs(double(i) - double(position)) < m_localSkip) {
results[i] = 1e17;
inliers[i] = 0;
linearErrors[i] = 1e17;
angularErrors[i] = 1e17;
continue;
}
OrientedPoint2D transform;
std::vector< std::pair<InterestPoint*, InterestPoint* > > correspondences;
gettimeofday(&start,NULL);
// std::cout << m_pointsReference[i].size() << " vs. " << pointsLocal.size() << std::endl;
results[i] = m_ransac->matchSets(m_pointsReference[i], pointsLocal, transform, correspondences);
gettimeofday(&end,NULL);
timersub(&end, &start, &diff);
timeradd(&ransacTime, &diff, &sum);
ransacTime = sum;
inliers[i] = correspondences.size();
OrientedPoint2D delta = m_posesReference[position] - transform;
linearErrors[i] = correspondences.size() ? delta * delta : 1e17;
angularErrors[i] = correspondences.size() ? delta.theta * delta.theta : 1e17;
}
for(unsigned int c = 0; c < 8; c++){
unsigned int maxCorres = 0;
double maxResult = 1e17;
double linError = 1e17, angError = 1e17;
double linErrorC = 1e17, angErrorC = 1e17;
double linErrorR = 1e17, angErrorR = 1e17;
bool valid = false;
for(unsigned int i = 0; i < m_pointsReference.size(); i++){
if(linError + angError > linearErrors[i] + angularErrors[i]) {
linError = linearErrors[i];
angError = angularErrors[i];
}
if(maxCorres < inliers[i]){
linErrorC = linearErrors[i];
angErrorC = angularErrors[i];
maxCorres = inliers[i];
}
if(maxResult > results[i]){
linErrorR = linearErrors[i];
angErrorR = angularErrors[i];
maxResult = results[i];
}
valid = valid || inliers[i] >= corresp[c];
}
if(valid){
m_match[c] += (linError <= (linErrorTh * linErrorTh) && angError <= (angErrorTh * angErrorTh) );
m_matchC[c] += (linErrorC <= (linErrorTh * linErrorTh) && angErrorC <= (angErrorTh * angErrorTh) );
m_matchR[c] += (linErrorR <= (linErrorTh * linErrorTh) && angErrorR <= (angErrorTh * angErrorTh) );
m_error[c] += sqrt(linError + angError);
m_errorC[c] += sqrt(linErrorC + angErrorC);
m_errorR[c] += sqrt(linErrorR + angErrorR);
m_valid[c]++;
}
}
}