double
FittingCurve2d::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double hint)
{
// evaluate hint
double param = hint;
double points[2];
nurbs.Evaluate (param, 0, 2, points);
Eigen::Vector2d p (points[0], points[1]);
Eigen::Vector2d r = p - pt;
double d_shortest_hint = r.squaredNorm ();
double d_shortest_elem (DBL_MAX);
// evaluate elements
std::vector<double> elements = pcl::on_nurbs::FittingCurve2d::getElementVector (nurbs);
double seg = 1.0 / (nurbs.Order () - 1);
for (unsigned i = 0; i < elements.size () - 1; i++)
{
double &xi0 = elements[i];
double &xi1 = elements[i + 1];
double dxi = xi1 - xi0;
for (unsigned j = 0; j < nurbs.Order (); j++)
{
double xi = xi0 + (seg * j) * dxi;
nurbs.Evaluate (xi, 0, 2, points);
p (0) = points[0];
p (1) = points[1];
r = p - pt;
double d = r.squaredNorm ();
if (d < d_shortest_elem)
{
d_shortest_elem = d;
param = xi;
}
}
}
if(d_shortest_hint < d_shortest_elem)
return hint;
else
return param;
}
ON_NurbsCurve
FittingCurve2d::initNurbsCurve2D (int order, const vector_vec2d &data, int ncps, double radiusF)
{
if (data.empty ())
printf ("[FittingCurve2d::initNurbsCurve2D] Warning, no boundary parameters available\n");
Eigen::Vector2d mean = NurbsTools::computeMean (data);
unsigned s = data.size ();
double r (0.0);
for (unsigned i = 0; i < s; i++)
{
Eigen::Vector2d d = data[i] - mean;
double sn = d.squaredNorm ();
if (sn > r)
r = sn;
}
r = radiusF * sqrt (r);
if (ncps < 2 * order)
ncps = 2 * order;
ON_NurbsCurve nurbs = ON_NurbsCurve (2, false, order, ncps);
nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1));
double dcv = (2.0 * M_PI) / (ncps - order + 1);
Eigen::Vector2d cv;
for (int j = 0; j < ncps; j++)
{
cv (0) = r * sin (dcv * j);
cv (1) = r * cos (dcv * j);
cv = cv + mean;
nurbs.SetCV (j, ON_3dPoint (cv (0), cv (1), 0.0));
}
return nurbs;
}
unsigned
NurbsTools::getClosestPoint (const Eigen::Vector2d &p, const Eigen::Vector2d &dir, const vector_vec2d &data,
unsigned &idxcp)
{
if (data.empty ())
throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");
size_t idx = 0;
idxcp = 0;
double dist2 (0.0);
double dist2cp (DBL_MAX);
for (size_t i = 0; i < data.size (); i++)
{
Eigen::Vector2d v = (data[i] - p);
double d2 = v.squaredNorm ();
if (d2 < dist2cp)
{
idxcp = i;
dist2cp = d2;
}
if (d2 == 0.0)
return i;
v.normalize ();
double d1 = dir.dot (v);
if (d1 / d2 > dist2)
{
idx = i;
dist2 = d1 / d2;
}
}
return idx;
}
double distance_squared () const { return translation.squaredNorm(); }
bool BeaconBasedPoseEstimator::m_kalmanAutocalibEstimator(LedGroup &leds,
double dt) {
auto const beaconsSize = m_beacons.size();
// Default measurement variance (for now, factor) per axis.
double varianceFactor = 1;
auto maxBoxRatio = m_params.boundingBoxFilterRatio;
auto minBoxRatio = 1.f / m_params.boundingBoxFilterRatio;
auto inBoundsID = std::size_t{0};
// Default to using all the measurements we can
auto skipBright = false;
{
auto totalLeds = leds.size();
auto identified = std::size_t{0};
auto inBoundsBright = std::size_t{0};
auto inBoundsRound = std::size_t{0};
for (auto const &led : leds) {
if (!led.identified()) {
continue;
}
identified++;
auto id = led.getID();
if (id >= beaconsSize) {
continue;
}
inBoundsID++;
if (led.isBright()) {
inBoundsBright++;
}
if (led.getMeasurement().knowBoundingBox) {
auto boundingBoxRatio =
led.getMeasurement().boundingBox.height /
led.getMeasurement().boundingBox.width;
if (boundingBoxRatio > minBoxRatio &&
boundingBoxRatio < maxBoxRatio) {
inBoundsRound++;
}
}
}
/// If we only see a few beacons, they may be as likely to send us spinning as
/// help us keep tracking.
#if 0
// Now we decide if we want to cut the variance artificially to
// reduce latency in low-beacon situations
if (inBoundsID < LOW_BEACON_CUTOFF) {
varianceFactor = 0.5;
}
#endif
if (inBoundsID - inBoundsBright >
DIM_BEACON_CUTOFF_TO_SKIP_BRIGHTS &&
m_params.shouldSkipBrightLeds) {
skipBright = true;
}
if (0 == inBoundsID) {
m_framesWithoutIdentifiedBlobs++;
} else {
m_framesWithoutIdentifiedBlobs = 0;
}
}
CameraModel cam;
cam.focalLength = m_camParams.focalLength();
cam.principalPoint = cvToVector(m_camParams.principalPoint());
ImagePointMeasurement meas{cam};
kalman::ConstantProcess<kalman::PureVectorState<>> beaconProcess;
const auto maxSquaredResidual =
m_params.maxResidual * m_params.maxResidual;
const auto maxZComponent = m_params.maxZComponent;
kalman::predict(m_state, m_model, dt);
/// @todo should we be recalculating this for each beacon after each
/// correction step? The order we filter them in is rather arbitrary...
Eigen::Matrix3d rotate =
Eigen::Matrix3d(m_state.getCombinedQuaternion());
auto numBad = std::size_t{0};
auto numGood = std::size_t{0};
for (auto &led : leds) {
if (!led.identified()) {
continue;
}
auto id = led.getID();
if (id >= beaconsSize) {
continue;
}
auto &debug = m_beaconDebugData[id];
debug.seen = true;
debug.measurement = led.getLocation();
if (skipBright && led.isBright()) {
continue;
}
// Angle of emission checking
// If we transform the body-local emission vector, an LED pointed
// right at the camera will be -Z. Anything with a 0 or positive z
// component is clearly out, and realistically, anything with a z
// component over -0.5 is probably fairly oblique. We don't want to
// use such beacons since they can easily introduce substantial
// error.
double zComponent =
(rotate * cvToVector(m_beaconEmissionDirection[id])).z();
if (zComponent > 0.) {
if (m_params.extraVerbose) {
std::cout << "Rejecting an LED at " << led.getLocation()
<< " claiming ID " << led.getOneBasedID()
<< std::endl;
}
/// This means the LED is pointed away from us - so we shouldn't
/// be able to see it.
led.markMisidentified();
/// @todo This could be a mis-identification, or it could mean
/// we're in a totally messed up state. Do we count this against
/// ourselves?
numBad++;
continue;
} else if (zComponent > maxZComponent) {
/// LED is too askew of the camera to provide reliable data, so
/// skip it.
continue;
}
#if 0
if (led.getMeasurement().knowBoundingBox) {
/// @todo For right now, if we don't have a bounding box, we're
/// assuming it's square enough (and only testing for
/// non-squareness on those who actually do have bounding
/// boxes). This is very much a temporary situation.
auto boundingBoxRatio =
led.getMeasurement().boundingBox.height /
led.getMeasurement().boundingBox.width;
if (boundingBoxRatio < minBoxRatio ||
boundingBoxRatio > maxBoxRatio) {
/// skip non-circular blobs.
numBad++;
continue;
}
}
#endif
auto localVarianceFactor = varianceFactor;
auto newIdentificationVariancePenalty =
std::pow(2.0, led.novelty());
/// Stick a little bit of process model uncertainty in the beacon,
/// if it's meant to have some
if (m_beaconFixed[id]) {
beaconProcess.setNoiseAutocorrelation(0);
} else {
beaconProcess.setNoiseAutocorrelation(
m_params.beaconProcessNoise);
kalman::predict(*(m_beacons[id]), beaconProcess, dt);
}
meas.setMeasurement(
Eigen::Vector2d(led.getLocation().x, led.getLocation().y));
led.markAsUsed();
auto state = kalman::makeAugmentedState(m_state, *(m_beacons[id]));
meas.updateFromState(state);
Eigen::Vector2d residual = meas.getResidual(state);
if (residual.squaredNorm() > maxSquaredResidual) {
// probably bad
numBad++;
localVarianceFactor *= m_params.highResidualVariancePenalty;
} else {
numGood++;
}
debug.residual.x = residual.x();
debug.residual.y = residual.y();
auto effectiveVariance =
localVarianceFactor * m_params.measurementVarianceScaleFactor *
newIdentificationVariancePenalty *
(led.isBright() ? BRIGHT_PENALTY : 1.) *
m_beaconMeasurementVariance[id] / led.getMeasurement().area;
debug.variance = effectiveVariance;
meas.setVariance(effectiveVariance);
/// Now, do the correction.
auto model =
kalman::makeAugmentedProcessModel(m_model, beaconProcess);
kalman::correct(state, model, meas);
m_gotMeasurement = true;
}
/// Probation: Dealing with ratios of bad to good residuals
bool incrementProbation = false;
if (0 == m_framesInProbation) {
// Let's try to keep a 3:2 ratio of good to bad when not "in
// probation"
incrementProbation = (numBad * 3 > numGood * 2);
} else {
// Already in trouble, add a bit of hysteresis and raising the bar
// so we don't hop out easily.
incrementProbation = numBad * 2 > numGood;
if (!incrementProbation) {
// OK, we're good again
m_framesInProbation = 0;
}
}
if (incrementProbation) {
m_framesInProbation++;
}
/// Frames without measurements: dealing with getting in a bad state
if (m_gotMeasurement) {
m_framesWithoutUtilizedMeasurements = 0;
} else {
if (inBoundsID > 0) {
/// We had a measurement, we rejected it. The problem may be the
/// plank in our own eye, not the speck in our beacon's eye.
m_framesWithoutUtilizedMeasurements++;
}
}
/// Output to the OpenCV state types so we can see the reprojection
/// debug view.
m_rvec = eiQuatToRotVec(m_state.getQuaternion());
cv::eigen2cv(m_state.position().eval(), m_tvec);
return true;
}
const CPoint3DWORLD CLandmark::_getOptimizedLandmarkIDWA( )
{
//ds return vector
CPoint3DWORLD vecPointXYZWORLD( Eigen::Vector3d::Zero( ) );
//ds total accumulated depth
double dInverseDepthAccumulated = 0.0;
//ds loop over all measurements
for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements )
{
//ds current inverse depth
const double dInverseDepth = 1.0/pMeasurement->vecPointXYZLEFT.z( );
//ds add current measurement with depth weight
vecPointXYZWORLD += dInverseDepth*pMeasurement->vecPointXYZWORLD;
//std::cout << "in camera frame: " << pMeasurement->vecPointXYZ.transpose( ) << std::endl;
//std::cout << "adding: " << dInverseDepth << " x " << pMeasurement->vecPointXYZWORLD.transpose( ) << std::endl;
//ds accumulate depth
dInverseDepthAccumulated += dInverseDepth;
}
//ds compute average point
vecPointXYZWORLD /= dInverseDepthAccumulated;
//std::cout << "from: " << vecPointXYZCalibrated.transpose( ) << " to: " << vecPointXYZWORLD.transpose( ) << std::endl;
++uOptimizationsSuccessful;
double dSumSquaredErrors = 0.0;
//ds loop over all previous measurements again
for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements )
{
//ds get point into camera (cast to avoid eclipse error..)
const CPoint3DHomogenized vecPointXYZLEFT( CMiniVisionToolbox::getHomogeneous( static_cast< CPoint3DCAMERA >( pMeasurement->matTransformationWORLDtoLEFT*vecPointXYZWORLD ) ) );
//ds get projected point
const CPoint2DHomogenized vecProjectionHomogeneous( m_matProjectionLEFT*vecPointXYZLEFT );
//ds compute pixel coordinates TODO remove cast
const Eigen::Vector2d vecUV = CWrapperOpenCV::getInterDistance( static_cast< Eigen::Vector2d >( vecProjectionHomogeneous.head< 2 >( )/vecProjectionHomogeneous(2) ), pMeasurement->ptUVLEFT );
//ds compute squared error
const double dSquaredError( vecUV.squaredNorm( ) );
//ds add up
dSumSquaredErrors += dSquaredError;
}
//ds average the measurement
dCurrentAverageSquaredError = dSumSquaredErrors/m_vecMeasurements.size( );
//ds if optimal
if( 5.0 > dCurrentAverageSquaredError )
{
bIsOptimal = false;
}
//ds return
return vecPointXYZWORLD;
}