double MIDDistanceCalculator::getMIDDistance(std::pair<std::vector<double>, std::vector<double> > const &aligned, size_t origSize1, size_t origSize2)
{
const std::vector<double> &alV1 = aligned.first;
const std::vector<double> &alV2 = aligned.second;
// printAlignedVectors(alV1, alV2);
double dist;
switch(distanceMeasure) {
case D_EUCLIDEAN:
dist = getEuclideanDistance(alV1, alV2);
break;
case D_CANBERRA:
dist = getCanberraDistance(alV1, alV2);
break;
case D_MANHATTAN:
dist = getManhattanDistance(alV1, alV2);
break;
case D_COSINE:
dist = getCosineCorrelation(alV1, alV2);
break;
case D_CUSTOM:
dist = getCustomDistance(alV1, alV2);
break;
default:
dist = getEuclideanDistance(alV1, alV2);
}
double divideBy = 1;
origSize1 = origSize1?alV1.size():origSize1;
origSize2 = origSize2?alV2.size():origSize2;
switch(MIDDistanceCalculator::distanceNormalization) {
case DN_MAX:
divideBy = std::max(origSize1, origSize2);
break;
case DN_MIN:
divideBy = std::min(origSize1, origSize2);
break;
case DN_PROD:
divideBy = origSize1 * origSize2;
break;
case DN_SUM:
divideBy = origSize1 + origSize2;
break;
}
// log(alV2.size())
return fabs(dist / divideBy);
}
void TimeWarp::calculateCausalChromaSimilarityMatrix(DoubleMatrix& firstChromaMatrix, DoubleMatrix& secondChromaMatrix, DoubleMatrix& simMatrix){
//calculates the chroma only similarity matrix
//but we have already done some, so is extending it...
int size = 0;
if (simMatrix.size() > 0){
size = simMatrix[0].size();
}
if (secondChromaMatrix.size() > size ){
for (int x = 0;x < firstChromaMatrix.size();x++){
if (x < simMatrix.size()){
//i.e. entries already exist
for (int y = (int)simMatrix[x].size();y < secondChromaMatrix.size();y++){
double distance;
if (useDotProduct)
distance = getChromaSimilarity(x, y, &firstChromaMatrix, &secondChromaMatrix);
else
distance = getEuclideanDistance(x, y, &firstChromaMatrix, &secondChromaMatrix);
printf("putting one back X %i Y %i dist %f \n", x, y, distance);
simMatrix[x].push_back(distance);
}
}
else {
DoubleVector d;
for (int y = 0;y < secondChromaMatrix.size();y++){
double distance;
if (useDotProduct)
distance = getChromaSimilarity(x, y, &firstChromaMatrix, &secondChromaMatrix);
else
distance = getEuclideanDistance(x, y, &firstChromaMatrix, &secondChromaMatrix);
printf("new row X %i Y %i dist %f\n", x, y, distance);
d.push_back( distance);
}
simMatrix.push_back(d);
}
}
}
if (size > 0)
printf("Causial CHROMA ONLY SIM SIZE %i x %i; ", (int)simMatrix.size(), (int) simMatrix[0].size());
printf("First matrix SIZE %i , SECOND size %i\n", (int)firstChromaMatrix.size(), (int) secondChromaMatrix.size());
}
void TimeWarp::calculatePartSimilarityMatrix(DoubleMatrix* firstChromaMatrix, DoubleMatrix* secondChromaMatrix, DoubleMatrix* simMatrix, int startX, int startY, int endX){
// printf("Calculate similarity : pointers : size %i x %i ", (int) firstChromaMatrix.size(), (int) secondChromaMatrix.size());
simMatrix->clear();
double distance, firstSum, secondSum;
endX = min (endX, (int)(*firstChromaMatrix).size()-1);//in case out of size
for (int x = startX;x <= endX;x++){
DoubleVector d;
for (int y = startY;y < (*secondChromaMatrix).size();y++){
if (useDotProduct)
distance = getChromaSimilarity(x, y, firstChromaMatrix, secondChromaMatrix);
else
distance = getEuclideanDistance(x, y, firstChromaMatrix, secondChromaMatrix);
d.push_back( distance);
} //end for y
(*simMatrix).push_back(d);
}//end for x
printf("..part sim size: %i, height: %i \n", (int) (*simMatrix).size(), (int) (*simMatrix)[0].size());
}//end self sim
void TimeWarp::calculateSimilarityMatrixWithPointers(DoubleMatrix* firstChromaMatrix, DoubleMatrix* secondChromaMatrix, DoubleMatrix* simMatrix){
printf("Calculate similarity : pointers : size %i x %i ", (int) (*firstChromaMatrix).size(), (int) (*secondChromaMatrix).size());
simMatrix->clear();
double distance, firstSum, secondSum;
for (int x = 0;x < (*firstChromaMatrix).size();x++){
DoubleVector d;
for (int y = 0;y < (*secondChromaMatrix).size();y++){
if (useDotProduct)
distance = getChromaSimilarity(x, y, firstChromaMatrix, secondChromaMatrix);
else
distance = getEuclideanDistance(x, y, firstChromaMatrix, secondChromaMatrix);
d.push_back( distance);
} //end for y
(*simMatrix).push_back(d);
}//end for x
printf("..sim size: %i, height: %i \n", (int) (*simMatrix).size(), (int) (*simMatrix)[0].size());
}//end self sim
hbrs_msgs::LaserScanSegmentList LaserScanSegmentation::getSegments(const sensor_msgs::LaserScan::ConstPtr &inputScan, bool store_data_points)
{
hbrs_msgs::LaserScanSegmentList segments;
vector<geometry_msgs::Point> data_points;
double dNumberofPointsBetweenStartAndEnd = 0;
unsigned int unSegmentStartPoint = 0;
unsigned int unSegmentEndPoint = 0;
uint32_t scan_size = ceil((inputScan->angle_max - inputScan->angle_min) / inputScan->angle_increment);
if(scan_size == 0)
return segments;
//run over laser scan data
for(unsigned int i = 0; i < (scan_size - 1); ++i)
{
++dNumberofPointsBetweenStartAndEnd;
// first point in laser scan is start point of the first segment
if(i == 0)
unSegmentStartPoint = i;
// the distance between two adjacent points is above a given threshold -> remember end point and start with a new segment
//cout << "angle: " << ppdLaserScan[i][BUFFER_COLUMN_YAW] << " " << (ppdLaserScan[i][BUFFER_COLUMN_YAW] * 180 / M_PI) << std::endl;
double dAngleCur = inputScan->angle_min + (i * inputScan->angle_increment);
double dDistanceCur = inputScan->ranges[i];
double dAngleNext = inputScan->angle_min + ((i+1) * inputScan->angle_increment);
double dDistanceNext = inputScan->ranges[i+1];
if(store_data_points)
{
geometry_msgs::Point cur_point;
cur_point.x = dDistanceCur * cos(dAngleCur);
cur_point.y = dDistanceCur * sin(dAngleCur);
data_points.push_back(cur_point);
}
// cout << "a: " << dAngleCur << " d: " << dDistanceCur << " a: " << dAngleNext<< " d: " << dAngleNext << endl;
if((getEuclideanDistance(dDistanceCur, dAngleCur, dDistanceNext, dAngleNext) > this->_dThresholdDistanceBetweenAdajecentPoints) || (i == (scan_size - 2)))
{
if( i < (scan_size - 2))
unSegmentEndPoint = i;
else
unSegmentEndPoint = i+1;
// if number of points between start and end point is lesser then 3 , it is not a segment
if(dNumberofPointsBetweenStartAndEnd >= this->_unMinimumPointsPerSegment )
{
geometry_msgs::Point centerPoint;
centerPoint = getCenterOfGravity(unSegmentStartPoint, unSegmentEndPoint, inputScan);
double dDistanceToSegment = sqrt( pow(centerPoint.x, 2.0) + pow(centerPoint.y, 2.0));
if(dDistanceToSegment < 5.0)
{
hbrs_msgs::LaserScanSegment seg;
seg.header = inputScan->header;
seg.header.stamp = ros::Time::now();
seg.center.x = centerPoint.x;
seg.center.y = centerPoint.y;
if(store_data_points)
seg.data_points = data_points;
/*
cout << "eucl: " << getEuclideanDistance(dDistanceCur, dAngleCur, dDistanceNext, dAngleNext) << " thr: " << this->_dThresholdDistanceBetweenAdajecentPoints << endl;
cout << "new segmented: " << endl;
cout << "dist: : " << dDistanceToSegment << endl;
*/
segments.segments.push_back(seg);
}
}
if( i < (scan_size - 2))
{
unSegmentStartPoint = i+1;
dNumberofPointsBetweenStartAndEnd = 0;
if(store_data_points)
data_points.clear();
}
}
}
segments.header = inputScan->header;
segments.header.stamp = ros::Time::now();
segments.num_segments = segments.segments.size();
return segments;
}
void TimeWarp::calculateRestrictedCausalChromaSimilarityMatrix(DoubleMatrix& firstChromaMatrix, DoubleMatrix& secondChromaMatrix, DoubleMatrix& simMatrix, const int& Xstart, const int& Ystart, const int& Xlimit, const int& yLimit){
//calculates the chroma only similarity matrix - used to reduce computation later when doing the joint energy and chroma matrix
DoubleVector d;
int size = 0;
if (simMatrix.size() > 0){
size = simMatrix[0].size();
}
if (secondChromaMatrix.size() > size ){
// printf("sim matrix size %i and second matrix size %i Xstart %i\n", simMatrix.size(), secondChromaMatrix.size(), Xstart);
for (int x = 0;x < Xstart;x++){
simMatrix[x].push_back(0);
}
int endX = min(Xlimit, (int)firstChromaMatrix.size());
for (int x = Xstart;x < endX;x++){
if (x < simMatrix.size()){
//i.e. entries already exist
for (int y = (int)simMatrix[x].size();y < secondChromaMatrix.size();y++){
double distance;
if (useDotProduct)
distance = getChromaSimilarity(x, y, &firstChromaMatrix, &secondChromaMatrix);
else
distance = getEuclideanDistance(x, y, &firstChromaMatrix, &secondChromaMatrix);
// printf("X %i Y %i dist %f\n", x, y, distance);
simMatrix[x].push_back(distance);
}
}
else {
DoubleVector d;
for (int y = 0;y < secondChromaMatrix.size();y++){
double distance;
if (useDotProduct)
distance = getChromaSimilarity(x, y, &firstChromaMatrix, &secondChromaMatrix);
else
distance = getEuclideanDistance(x, y, &firstChromaMatrix, &secondChromaMatrix);
// printf("new row X %i Y %i dist %f\n", x, y, distance);
d.clear();
d.push_back( distance);
}
simMatrix.push_back(d);
}
}//end for good section
for (int x = endX;x < firstChromaMatrix.size(); x++){
if (x < simMatrix.size()){
simMatrix[x].push_back(0);
}else{
d.clear();
d.push_back(0);
simMatrix.push_back(d);
}
}//end for x to end
}
// if (size > 0)
// printf("Causial CHROMA ONLY SIM SIZE %i x %i; ", (int)simMatrix.size(), (int) simMatrix[0].size());
//printf("First matrix SIZE %i , SECOND size %i\n", (int)firstChromaMatrix.size(), (int) secondChromaMatrix.size());
}
