double signedDistanceInsideConvexHull( const Ref<const Matrix<double, 2, Dynamic>> &pts, const Ref<const Vector2d> &q) { std::vector<Point> hull_pts = convexHull(eigenToPoints(pts)); double d_star = std::numeric_limits<double>::infinity(); Matrix2d R; R << 0, 1, -1, 0; for (int i = 0; i < (static_cast<int>(hull_pts.size()) - 1); ++i) { Vector2d ai = R * Vector2d(hull_pts[i + 1].x - hull_pts[i].x, hull_pts[i + 1].y - hull_pts[i].y); double b = ai.transpose() * Vector2d(hull_pts[i].x, hull_pts[i].y); double n = ai.norm(); if (std::isnormal(n)) { ai = ai.array() / n; b = b / n; double d = b - ai.transpose() * q; // std::cout << "pt0: " << hull_pts[i].x << " " << hull_pts[i].y << // std::endl; // std::cout << "pt1: " << hull_pts[i+1].x << " " << hull_pts[i+1].y << // std::endl; // std::cout << "ai: " << ai.transpose() << std::endl; // std::cout << "b: " << b << std::endl; // std::cout << "d: " << d << std::endl; if (d < d_star) { d_star = d; } } } return d_star; }
bool AStarPlanner::computeAStarIn2DGrid( Vector2d source, Vector2d target ) { PlanCell* startCell = dynamic_cast<PlanCell*>(grid_->getCell(source)); if( startCell == NULL ) { cout << "start (" << source.transpose() << ") not in grid" << endl; return false; } PlanCell* goalCell = dynamic_cast<PlanCell*>(grid_->getCell(target)); if( goalCell == NULL ) { cout << "goal (" << target.transpose() << ") not in grid" << endl; return false; } Vector2i startCoord = startCell->getCoord(); cout << "Start Pos = (" << source[0] << " , " << source[1] << ")" << endl; cout << "Start Coord = (" << startCoord[0] << " , " << startCoord[1] << ")" << endl; Vector2i goalCoord = goalCell->getCoord(); cout << "Goal Pos = (" << target[0] << " , " << target[1] << ")" << endl; cout << "Goal Coord = (" << goalCoord[0] << " , " << goalCoord[1] << ")" << endl; if( startCoord == goalCoord ) { cout << " no planning as cells are identical" << endl; return false; } PlanState* start = new PlanState( startCell, grid_ ); PlanState* goal = new PlanState( goalCell, grid_ ); if ( start == NULL || goal == NULL ) { cout << "Start or goal == NULL" << endl; return false; } if( solveAStar( start, goal ) ) { double SumOfCost= 0.0; for(int i=0; i< int(path_.size()); i++ ) { //cout << "Cell "<< i <<" = " << endl << path_[i] << endl; SumOfCost += dynamic_cast<PlanCell*>(cell_path_[i])->getCost(); } cout << " SumOfCost = " << SumOfCost << endl; return true; } else { return false; } }
void addPoint() { Vector2d v; string m1,m2; string s = a[2]; double x,y; int pos; pos=s.find(',',0); if(pos!=std::string::npos) { m1 = s.substr(0,pos); m2 = s.substr(pos+1,s.size()-pos); x = stringToNum<double>(m1); y = stringToNum<double>(m2); } else { cout<<"direction error please write like this"<<endl; cout<<"p2 1 5,6"<<endl; exit(0); } v<<x,y; point[pcnt].setPoint(v); point[pcnt].setName(a[0]); pcnt++; cout<<"add a dot :"<<a[0]<<endl; cout<<"location is:"<<v.transpose()<<endl; }
bool operator()(const pair<CircleEvent*,Vector2d>& lhs_, const pair<CircleEvent*,Vector2d>&rhs_){ static const bool dbg = 0; Vector2d lhs = lhs_.second, rhs = rhs_.second; if(dbg) cout << "inputs: " << lhs.transpose() << ", " << rhs.transpose() << endl; if(rhs(0) - lhs(0) > 1e-5) { if(dbg) printf("Returning true: 0 index is smaller\n"); return true; } else if((fabs(lhs(0) - rhs(0)) < 1e-5) && ((rhs(1) - lhs(1)) > 1e-5)) { if(dbg) printf("Returning true: 0 is same, 1 index is smaller\n"); return true; } if(dbg) printf("Returning false\n"); return false; }
/* ******************************************************************************************** */ void termination () { sweepLine -= 3.0; vector <AVL<TreeNode*>::Node*> nodes; avl.traversal(nodes); for(int i = 0; i < nodes.size(); i++) { printf("awef\n"); TreeNode* node = nodes[i]->value; Vector2d temp; node->value(&temp); cout << "\t" << temp.transpose() << endl; node->edge1->p1 = temp; node->edge2->p0 = temp; } }
void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){ /////////////////////////////////// // we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one CovarianceEstimator ce(_graph); ce.setVertices(vset); ce.setGauge(_lastVertex); ce.compute(); assert(!_lastVertex->fixed() && "last Vertex is fixed"); assert(_firstRobotPose->fixed() && "first Vertex is not fixed"); OptimizableGraph::VertexSet tmpvset = vset; for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){ VertexSE2 *vertex = (VertexSE2*) *it; MatrixXd Pv = ce.getCovariance(vertex); Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1); SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate(); Vector2d hxy (delta.translation().x(), delta.translation().y()); double perceptionRange =1; if (hxy.x()-perceptionRange>0) hxy.x() -= perceptionRange; else if (hxy.x()+perceptionRange<0) hxy.x() += perceptionRange; else hxy.x() = 0; if (hxy.y()-perceptionRange>0) hxy.y() -= perceptionRange; else if (hxy.y()+perceptionRange<0) hxy.y() += perceptionRange; else hxy.y() = 0; double d2 = hxy.transpose() * Pxy.inverse() * hxy; if (d2 > 5.99) vset.erase(*it); } }
/* * frame_offset: used to place HxBj in right place in H */ bool MSCKF::getResidualH(VectorXd& ri, MatrixXd& Hi, Vector3d feature_pose, MatrixXd measure, MatrixXd pose_mtx, int frame_offset) { int num_frame = (int)pose_mtx.cols(); int errorStateLength = (int)fullErrorCovariance.rows(); ri = VectorXd::Zero(2 * num_frame); Hi = MatrixXd::Zero(2 * num_frame, errorStateLength); // Hi cols == error state length MatrixXd HxBj, Hc; MatrixXd Mij, tmp39; MatrixXd Hfi; MatrixXd Hf; // use double precision to increase numerial result Hfi = MatrixXd::Zero(2 * num_frame, 3); for(int j = 0; j < num_frame; j++) { cout << "frame is " << frame_offset+j << endl; Matrix3d R_gb = quaternion_to_R(pose_mtx.block<4, 1>(0, j)); //double xx = pts[i * 3 + 0] - position(0); //double yy = pts[i * 3 + 1] - position(1); //double zz = pts[i * 3 + 2] - position(2); //Vector3d local_point = Ric.inverse() * (quat.inverse() * Vector3d(xx, yy, zz) - Tic); Vector3d feature_in_c = R_cb * R_gb.transpose() * (feature_pose - pose_mtx.block<3, 1>(4, j)) + fullNominalState.segment(16, 3); //Vector2d projPtr = projectPoint(feature_pose, R_gb, pose_mtx.block<3, 1>(4, j), fullNominalState.segment(16, 3)); //zij = cam.h(R_cb * R_gb.transpose() * (feature_pose - p_gb) + p_cb); Vector2d projPtr = projectCamPoint(feature_in_c); //cout << "projPtr projectPoint" << projPtr.transpose() << endl; /* if (feature_in_c(2) < 1e-4) { projPtr = measure.col(j); return false; } else { projPtr = projectCamPoint(feature_in_c); }*/ //if (projPtr(0)<0 || projPtr(1)>800 || projPtr(1)<0||projPtr(1)>800) // return false; // cout << "projPtr" << projPtr.transpose() << endl; cout << "measure is " << measure.col(j).transpose() << endl; cout << "feature in c is " << feature_in_c.transpose() << endl; cout << "estimat is " << projPtr.transpose() << endl; ri.segment(j * 2, 2) = measure.col(j) - projPtr; // cout << "ri piece is" << ri.segment(j * 2, 2) <<endl; if (ri.segment(j * 2, 2).norm() > 2.0) { return false; } Mij = cam.Jh(feature_in_c) * R_cb * R_gb.transpose(); tmp39 = MatrixXd::Zero(3, 9); tmp39.block<3, 3>(0, 0) = skew_mtx(feature_pose - pose_mtx.block<3, 1>(4, j)); tmp39.block<3, 3>(0, 3) = -Matrix3d::Identity(); HxBj = Mij * tmp39; // 2x9 Hc = cam.Jh(feature_in_c); // 2x3 // cout << "Mij is " << endl << Mij << endl; // cout << "tmp39 is " << endl << tmp39 << endl; // cout << "HxBj is " << endl << HxBj << endl; // cout << "Hc is " << endl << Hc << endl; Hi.block<2, 9>(j * 2, ERROR_STATE_SIZE + 3 + ERROR_POSE_STATE_SIZE * (frame_offset + j)) = HxBj; Hi.block<2, 3>(j * 2, ERROR_STATE_SIZE) = Hc; Hf = cam.Jh(feature_in_c) * R_cb * R_gb.transpose(); Hfi.block<2, 3>(j * 2, 0) = Hf.cast<double>(); } // now carry out feature error marginalization JacobiSVD<MatrixXd> svd(Hfi.transpose(), ComputeFullV); MatrixXd left_null = svd.matrixV().cast<double>().rightCols(2 * num_frame - 3).transpose(); // MatrixXd S = svd.singularValues().asDiagonal(); // MatrixXd U = svd.matrixU(); // MatrixXd V = svd.matrixV(); // MatrixXd D = Hfi.transpose()-U*S*V.transpose(); // std::cout << "\n" << D.norm() << " " << sqrt((D.adjoint()*D).trace()) << "\n"; // printf("left null size (%d, %d)\n", left_null.rows(), left_null.cols()); // printf("ri size (%d, %d)\n", ri.rows(), ri.cols()); // printf("Hi size (%d, %d)\n", Hi.rows(), Hi.cols()); // cout << "before left null" << endl; // cout << "------------------" << endl; // cout << ri << endl; // cout << Hi << endl; ri = left_null * ri; Hi = left_null * Hi; // cout << "one measure, one H" << endl; // cout << "------------------" << endl; // cout << ri << endl; // cout << Hi << endl; // printf("ri size (%d, %d)\n", ri.rows(), ri.cols()); // printf("Hi size (%d, %d)\n", Hi.rows(), Hi.cols()); double gamma; MatrixXd tmpK = Hi * fullErrorCovariance * Hi.transpose(); MatrixXd tmpKinv = tmpK.colPivHouseholderQr().solve(MatrixXd::Identity(tmpK.rows(), tmpK.cols())); int k = ri.size(); gamma = fabs(ri.dot( tmpKinv * ri)); cout << "gamma " << gamma<< endl; // cout << "ha? " << fabs(ri.dot( Hi *Hi.transpose()* ri))<< endl; if (k>30) k = 30; if (gamma*gamma > chi_test[k]) return false; else return true; }