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;
}
