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;
}
Transformation<double> StereoCartography::estimateOdometry(const vector<Feature> & featureVec)
{
//Matching
int numLandmarks = LM.size();
int numActive = min(300, numLandmarks);
vector<Feature> lmFeatureVec;
// cout << "ca va" << endl;
for (unsigned int i = numLandmarks - numActive; i < numLandmarks; i++)
{
lmFeatureVec.push_back(Feature(Vector2d(0, 0), LM[i].d));
}
// cout << "ca va" << endl;
Matcher matcher;
vector<int> matchVec;
matcher.bruteForce(featureVec, lmFeatureVec, matchVec);
Odometry odometry(trajectory.back(), stereo.TbaseCam1, stereo.cam1);
// cout << "ca va" << endl;
for (unsigned int i = 0; i < featureVec.size(); i++)
{
const int match = matchVec[i];
if (match == -1) continue;
odometry.observationVec.push_back(featureVec[i].pt);
odometry.cloud.push_back(LM[numLandmarks - numActive + match].X);
}
// cout << "cloud : " << odometry.cloud.size() << endl;
//RANSAC
odometry.Ransac();
// cout << odometry.TorigBase << endl;
//Final transformation computation
odometry.computeTransformation();
// cout << odometry.TorigBase << endl;
return odometry.TorigBase;
}
static inline Vector2d IMAGE_TO_BODY_COORDS (double x, double y) {
return Vector2d(x - 0.5, y - 0.36335788) * LANDER_WIDTH();
}
#include <catch.hpp>
#include <set>
#include <knnclassifier.h>
using Eigen::Vector2d;
using Eigen::VectorXd;
using std::set;
using std::vector;
TEST_CASE("K Nearest Neighbors classifier") {
vector<VectorXd> x {
Vector2d(1.0, 1.0),
Vector2d(1.0, 2.0),
Vector2d(1.5, 3.5),
Vector2d(1.5, 5.5),
Vector2d(1.5, 8.0),
Vector2d(2.0, 1.0),
Vector2d(2.0, 2.5),
Vector2d(2.5, 1.5),
Vector2d(2.5, 8.0),
Vector2d(3.0, 2.5),
Vector2d(3.0, 3.0),
Vector2d(3.5, 6.5),
Vector2d(4.0, 1.5),
Vector2d(4.0, 4.5),
Vector2d(4.5, 4.0),
Vector2d(5.0, 5.5),
Vector2d(5.0, 8.0),
Vector2d(6.0, 4.0),
