int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray &q_out) { Eigen::Matrix4f b = KDLToEigenMatrix(p_in); std::vector<std::vector<double> > solution_ik; if(free_angle_ == 0) { ROS_DEBUG("Solving with %f",q_init(0)); pr2_arm_ik_.computeIKShoulderPan(b,q_init(0),solution_ik); } else { pr2_arm_ik_.computeIKShoulderRoll(b,q_init(2),solution_ik); } if(solution_ik.empty()) return -1; double min_distance = 1e6; int min_index = -1; for(int i=0; i< (int) solution_ik.size(); i++) { ROS_DEBUG("Solution : %d",(int)solution_ik.size()); for(int j=0; j < (int)solution_ik[i].size(); j++) { ROS_DEBUG("%d: %f",j,solution_ik[i][j]); } ROS_DEBUG(" "); ROS_DEBUG(" "); double tmp_distance = computeEuclideanDistance(solution_ik[i],q_init); if(tmp_distance < min_distance) { min_distance = tmp_distance; min_index = i; } } if(min_index > -1) { q_out.resize((int)solution_ik[min_index].size()); for(int i=0; i < (int)solution_ik[min_index].size(); i++) { q_out(i) = solution_ik[min_index][i]; } return 1; } else return -1; }
bool KNN::predict(VectorDouble inputVector,UINT K){ if( !trained ){ errorLog << "predict(VectorDouble inputVector,UINT K) - KNN model has not been trained" << endl; return false; } if( inputVector.size() != numFeatures ){ errorLog << "predict(VectorDouble inputVector) - the size of the input vector " << inputVector.size() << " does not match the number of features " << numFeatures << endl; return false; } if( K > trainingData.getNumSamples() ){ errorLog << "predict(VectorDouble inputVector,UINT K) - K Is Greater Than The Number Of Training Samples" << endl; return false; } if( useScaling ){ for(UINT i=0; i<inputVector.size(); i++){ inputVector[i] = scale(inputVector[i], ranges[i].minValue, ranges[i].maxValue, 0, 1); } } //TODO - need to build a kdtree of the training data to allow better realtime prediction const UINT M = trainingData.getNumSamples(); vector< IndexedDouble > neighbours; for(UINT i=0; i<M; i++){ double dist = 0; UINT classLabel = trainingData[i].getClassLabel(); VectorDouble trainingSample = trainingData[i].getSample(); switch( distanceMethod ){ case EUCLIDEAN_DISTANCE: dist = computeEuclideanDistance(inputVector,trainingSample); break; case COSINE_DISTANCE: dist = computeCosineDistance(inputVector,trainingSample); break; case MANHATTAN_DISTANCE: dist = computeManhattanDistance(inputVector, trainingSample); break; default: errorLog << "predict(vector< double > inputVector) - unkown distance measure!" << endl; return false; break; } if( neighbours.size() < K ){ neighbours.push_back( IndexedDouble(classLabel,dist) ); }else{ //Find the maximum value in the neighbours buffer double maxValue = neighbours[0].value; UINT maxIndex = 0; for(UINT n=1; n<neighbours.size(); n++){ if( neighbours[n].value > maxValue ){ maxValue = neighbours[n].value; maxIndex = n; } } //If the dist is less than the maximum value in the buffer, then replace that value with the new dist if( dist < maxValue ){ neighbours[ maxIndex ] = IndexedDouble(classLabel,dist); } } } //Predict the class ID using the labels of the K nearest neighbours if( classLikelihoods.size() != numClasses ) classLikelihoods.resize(numClasses,0); else for(UINT i=0; i<classLikelihoods.size(); i++){ classLikelihoods[i] = 0; } if( classDistances.size() != numClasses ) classDistances.resize(numClasses,0); else for(UINT i=0; i<classDistances.size(); i++){ classDistances[i] = 0; } //Count the classes for(UINT k=0; k<neighbours.size(); k++){ UINT classLabel = neighbours[k].index; if( classLabel == 0 ){ errorLog << "predict(VectorDouble inputVector) - Class label of training example can not be zero!" << endl; return false; } //Find the index of the classLabel UINT classLabelIndex = 0; for(UINT j=0; j<numClasses; j++){ if( classLabel == classLabels[j] ){ classLabelIndex = j; break; } } classLikelihoods[ classLabelIndex ] += 1; classDistances[ classLabelIndex ] += neighbours[k].value; } //Get the max count double maxCount = classLikelihoods[0]; UINT maxIndex = 0; for(UINT i=1; i<classLikelihoods.size(); i++){ if( classLikelihoods[i] > maxCount ){ maxCount = classLikelihoods[i]; maxIndex = i; } } //Compute the average distances per class for(UINT i=0; i<classDistances.size(); i++){ if( classLikelihoods[i] > 0 ) classDistances[i] /= classLikelihoods[i]; else classDistances[i] = BIG_DISTANCE; } //Normalize the likelihoods for(UINT i=0; i<classLikelihoods.size(); i++){ classLikelihoods[i] /= double( neighbours.size() ); } //Set the maximum likelihood value maxLikelihood = classLikelihoods[ maxIndex ]; if( useNullRejection ){ if( classDistances[ maxIndex ] <= rejectionThresholds[ maxIndex ] ){ predictedClassLabel = classLabels[maxIndex]; }else{ predictedClassLabel = GRT_DEFAULT_NULL_CLASS_LABEL; //Set the gesture label as the null label } }else{ predictedClassLabel = classLabels[maxIndex]; } return true; }
void SonarMap::addScan(double sonar_x, double sonar_y, double sonar_theta, double fov, double max_range, double distance, double uncertainty) { int cx=0, cy=0; double wx = 0.0, wy = 0.0; convertToCell(sonar_x, sonar_y, cx, cy); mapstore::MapStoreCone msc(double(cx), double(cy), sonar_theta, fov, distance/resolution_); std::list<int> occxs; std::list<int> occys; std::list<double> occ_vals; double sum_occupied = 0; if (distance<uncertainty/2) { //fixes crashing on wall entering return; } while (msc.nextCell(cx,cy)) { if (convertToMap(cx, cy, wx, wy)) { double tmp_lin_dist = computeEuclideanDistance(wx, wy, sonar_x, sonar_y); double tmp_ang_dist = computeAngularDistance(sonar_theta, atan2(wy-sonar_y, wx-sonar_x)); double p_linear; double p_angular = this->Ea(fov, tmp_ang_dist); double tmp_occ = 0; double tmp_free = 0; if (abs(distance-max_range)<uncertainty) { //beams do not hit any obstacle p_linear = this->ErFree(distance, tmp_lin_dist, uncertainty); tmp_free = p_linear*p_angular; } else { if (tmp_lin_dist<distance) { p_linear = this->ErFree(distance, tmp_lin_dist, uncertainty); tmp_free = p_linear*p_angular; } else { double p_linear = this->ErOcc(distance, tmp_lin_dist, uncertainty); tmp_occ = p_linear*p_angular; } } double free_val_prev = map_free_.get(cx,cy); double occ_val_prev = map_occupied_.get(cx,cy); double free_val_new = free_val_prev + tmp_free - free_val_prev*tmp_free; double occ_val_new = tmp_occ*(1-free_val_new); sum_occupied += occ_val_new; occ_vals.push_front(occ_val_new); occxs.push_front(cx); occys.push_front(cy); map_free_.set(cx,cy, free_val_new); } } //Normalization pass over stored x and y's while (!occ_vals.empty()) { cx = occxs.back(); cy = occys.back(); double tmp_val = occ_vals.back(); if (sum_occupied>0.05) { //normalization not possible otherwise double normalized_occ = tmp_val/sum_occupied; double occ_prev = map_occupied_.get(cx,cy); map_occupied_.set(cx,cy, occ_prev + normalized_occ - occ_prev*normalized_occ); } double occ = map_occupied_.get(cx,cy); double free = map_free_.get(cx,cy); if (occ>free) { map_.set(cx, cy, occ); } else { map_.set(cx,cy, -free); } occxs.pop_back(); occys.pop_back(); occ_vals.pop_back(); } }