// @from http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00173.html
static bool pinv(const Eigen::Matrix2d& a, Eigen::Matrix2d& a_pinv)
{
// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
if (a.rows() < a.cols())
return false;
// SVD
Eigen::JacobiSVD<Eigen::Matrix2d> svdA(a, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector2d vSingular = svdA.singularValues();
// Build a diagonal matrix with the Inverted Singular values
// The pseudo inverted singular matrix is easy to compute :
// is formed by replacing every nonzero entry by its reciprocal (inversing).
Eigen::Vector2d vPseudoInvertedSingular(svdA.matrixV().cols(),1);
for (int iRow = 0; iRow < vSingular.rows(); iRow++)
{
if (fabs(vSingular(iRow)) <= 1e-10) // Todo : Put epsilon in parameter
{
vPseudoInvertedSingular(iRow, 0) = 0.;
}
else
{
vPseudoInvertedSingular(iRow, 0) = 1. / vSingular(iRow);
}
}
// A little optimization here
Eigen::Matrix2d mAdjointU = svdA.matrixU().adjoint().block(0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols());
// Pseudo-Inversion : V * S * U'
a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
return true;
}
void SelfRobot::selfLandmarkDataCallback(const read_omni_dataset::LRMLandmarksData::ConstPtr& landmarkData, int RobotNumber)
{
ROS_WARN(" got landmark data from self robot (ID=%d)",RobotNumber);
uint seq = landmarkData->header.seq;
// There are a total of 10 distinct, known and static landmarks in this dataset.
for(int i=0;i<10; i++)
{
if(landmarkData->found[i])
{
///Below is the procedure to calculate the observation covariance associate with the ball measurement made by the robots. Caution: Make sure the validity of the calculations below by crosschecking the obvious things, e.g., covariance cannot be negative or very close to 0
Eigen::Vector2d tempLandmarkObsVec = Eigen::Vector2d(landmarkData->x[i],landmarkData->y[i]);
double d = tempLandmarkObsVec.norm(),
phi = atan2(landmarkData->y[i],landmarkData->x[i]);
double covDD = (K1*fabs(1.0-(landmarkData->AreaLandMarkActualinPixels[i]/landmarkData->AreaLandMarkExpectedinPixels[i])))*(d*d);
double covPhiPhi = K2*(1/(d+1));
double covXX = pow(cos(phi),2) * covDD
+ pow(sin(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
double covYY = pow(sin(phi),2) * covDD
+ pow(cos(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
ROS_INFO("Landmark %d found in the image, refer to the method to see how covariances are calculated",i);
}
}
}
void SelfRobot::selfTargetDataCallback(const read_omni_dataset::BallData::ConstPtr& ballData, int RobotNumber)
{
ROS_WARN("Got ball data from self robot %d",RobotNumber);
Time curObservationTime = ballData->header.stamp;
if(ballData->found)
{
///Below is the procedure to calculate the observation covariance associate with the ball measurement made by the robots. Caution: Make sure the validity of the calculations below by crosschecking the obvious things, e.g., covariance cannot be negative or very close to 0
Eigen::Vector2d tempBallObsVec = Eigen::Vector2d(ballData->x,ballData->y);
double d = tempBallObsVec.norm(),
phi = atan2(ballData->y,ballData->x);
double covDD = (double)(1/ballData->mismatchFactor)*(K3*d + K4*(d*d));
double covPhiPhi = K5*(1/(d+1));
double covXX = pow(cos(phi),2) * covDD
+ pow(sin(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
double covYY = pow(sin(phi),2) * covDD
+ pow(cos(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
ROS_INFO("Ball found in the image, refer to the method to see how covariances are calculated");
}
else
{
ROS_INFO("Ball not found in the image");
}
}
Eigen::Vector2d CircularGroundPath::RelativePositionCalculator::localCoordinates( Eigen::Vector3d const& _center, Eigen::Vector2d global_coordinates )
{
Eigen::Vector2d local;
local.x() = global_coordinates.x() - _center.x();
local.y() = global_coordinates.y() - _center.y();
return local;
}
/**
* Computes the undistorted image coordinates of an input pixel via
* bilinear interpolation of its integer-coordinate 4-neighboors.
*
* \param dist_uv input distorted pixel coordinates.
* \param rect_uv output parameter.
*/
void rectifyBilinearLookup(const Eigen::Vector2d& dist_uv,
Eigen::Vector2d* rect_uv) const {
int u = (int)dist_uv.x();
int v = (int)dist_uv.y();
assert(u >= 0 && v >= 0 && u < _input_camera.width && v < _input_camera.height);
// weights
float wright = (dist_uv.x() - u);
float wbottom = (dist_uv.y() - v);
float w[4] = {
(1 - wright) * (1 - wbottom),
wright * (1 - wbottom),
(1 - wright) * wbottom,
wright * wbottom
};
int ra_index = v * _input_camera.width + u;
uint32_t neighbor_indices[4] = {
ra_index,
ra_index + 1,
ra_index + _input_camera.width,
ra_index + _input_camera.width + 1
};
rect_uv->x() = 0;
rect_uv->y() = 0;
for(int i = 0; i < 4; ++i) {
rect_uv->x() += w[i] * _map_x[neighbor_indices[i]];
rect_uv->y() += w[i] * _map_y[neighbor_indices[i]];
}
}
bool ProbabilisticStereoTriangulator<CAMERA_GEOMETRY_T>::computeReprojectionError4(
const std::shared_ptr<okvis::MultiFrame>& frame, size_t camId,
size_t keypointId, const Eigen::Vector4d& homogeneousPoint,
double& outError) const {
OKVIS_ASSERT_LT_DBG(Exception, keypointId, frame->numKeypoints(camId),
"Index out of bounds");
Eigen::Vector2d y;
okvis::cameras::CameraBase::ProjectionStatus status = frame
->geometryAs<CAMERA_GEOMETRY_T>(camId)->projectHomogeneous(
homogeneousPoint, &y);
if (status == okvis::cameras::CameraBase::ProjectionStatus::Successful) {
Eigen::Vector2d k;
Eigen::Matrix2d inverseCov = Eigen::Matrix2d::Identity();
double keypointStdDev;
frame->getKeypoint(camId, keypointId, k);
frame->getKeypointSize(camId, keypointId, keypointStdDev);
keypointStdDev = 0.8 * keypointStdDev / 12.0;
inverseCov *= 1.0 / (keypointStdDev * keypointStdDev);
y -= k;
outError = y.dot(inverseCov * y);
return true;
} else
return false;
}
void
LaserSensorModel2D::setInformationForVertexPoint(EdgeSE2PointXYCov*& edge, VertexPointXYCov*& point, LaserSensorParams& params)
{
double beamAngle = atan2(edge->measurement()(1), edge->measurement()(0));
///calculate incidence angle in point frame
Eigen::Vector2d beam = edge->measurement();
Eigen::Vector2d normal = point->normal();
beam = beam.normalized();
normal = normal.normalized();
double incidenceAngle = 0.0;
if(point->covariance() != Eigen::Matrix2d::Identity())
incidenceAngle = acos(normal.dot(beam));
double d = (tan(params.angularResolution * 0.5) * edge->measurement().norm());
///uncertainty of the surface measurement in direction of the beam
double dk = fabs(params.scale * (d / cos(incidenceAngle)));
edge->information().setIdentity();
edge->information()(0,0) = 1.0 / ((dk + params.sensorPrecision) * (dk + params.sensorPrecision));
double cError = 0.001 * edge->measurement().norm();
edge->information()(1,1) = 1.0 / (cError * cError);
Eigen::Rotation2Dd rot(beamAngle);
Eigen::Matrix2d mrot = rot.toRotationMatrix();
edge->information() = mrot * edge->information() * mrot.transpose();
}
std::vector<cv::DMatch> Registrator3P::getInliers(const ptam::KeyFrame& kfa, const ptam::KeyFrame& kfb,
const std::vector<cv::DMatch>& matches,
const Sophus::SE3d& relPoseAB,
double threshold,
std::vector<Observation>& obs)
{
Sophus::SE3d relPoseBA = relPoseAB.inverse();
std::vector<cv::DMatch> inliers;
double thresh2 = threshold*threshold;
for (uint i = 0; i < matches.size(); i++) {
const cv::DMatch& m = matches[i];
const Eigen::Vector3d& mpa = cs_geom::toEigenVec(kfa.mapPoints[m.queryIdx]->v3RelativePos);
cv::Point2f imbcv;
int scale = 1;// << kfb.keypoints[m.trainIdx].octave;
imbcv.x = kfb.keypoints[m.trainIdx].pt.x * scale;
imbcv.y = kfb.keypoints[m.trainIdx].pt.y * scale;
Eigen::Vector2d ipb(imbcv.x, imbcv.y);
Eigen::Vector2d err = cam_[kfb.nSourceCamera].project3DtoPixel(relPoseBA*mpa) - ipb;;
if (err.dot(err) < thresh2) {
inliers.push_back(m);
obs.push_back(Observation(mpa, cam_[kfb.nSourceCamera].unprojectPixel(ipb)));
}
}
return inliers;
}
void drawEllipse(cv::Mat & im, const Eigen::Matrix2d & A, const Eigen::Vector2d & b, const double sigma, const cv::Scalar col, const int thickness, const bool bMarkCentre)
{
if(bMarkCentre)
{
const cv::Point centre(cvRound(b.x()), cvRound(b.y()));
cv::circle(im, centre, 2, col, -1);
}
const int nNumPoints = 100;
const Eigen::Matrix2d rootA = matrixSqrt(A);
cv::Point p_last;
for(int i=0;i<nNumPoints;i++)
{
const double dTheta = i*(2*M_PI/nNumPoints);
const Eigen::Vector2d p1 (sin(dTheta), cos(dTheta));
const Eigen::Vector2d p_sigma = p1 * sigma;
const Eigen::Vector2d p_trans = rootA*p_sigma + b;
const cv::Point p_im(cvRound(p_trans.x()), cvRound(p_trans.y()));
if(i>0)
cv::line(im, p_last, p_im, col, thickness);
p_last = p_im;
}
}
void ConvexPolygon::calculateMatrixForm() {
//every two adjacent endpoints define a line -> inequality constraint
//first, resize A and b. x and b are column vectors
this->A = Eigen::MatrixXd (this->endpoints.size(),2);
this->b = Eigen::VectorXd (this->endpoints.size());
Eigen::Vector2d normal;
for(size_t i=1; i<endpoints.size(); i++) {
//to define the correct line direction, we also need a point on the inside of the constraint - the seed
normal(0) = endpoints[i-1](1) - endpoints[i](1);
normal(1) = endpoints[i](0) - endpoints[i-1](0);
if(normal.dot(seed) > 0) { //we want the outward pointing normal, so n.dot(s) < 0
normal = -normal;
}
normal.normalize();
b(i-1) = -endpoints[i].dot(normal); //endpoints[i];
A(i-1,0) = normal(0);
A(i-1,1) = normal(1);
}
normal(0) = endpoints.back()(1) - endpoints.front()(1);
normal(1) = endpoints.front()(0) - endpoints.back()(0);
if(normal.dot(seed) > 0) { //we want the outward pointing normal, so n.dot(s) < 0
normal = -normal;
}
normal.normalize();
b(endpoints.size()-1) = -endpoints.front().dot(normal); //endpoints[i];
A(endpoints.size()-1,0) = normal(0);
A(endpoints.size()-1,1) = normal(1);
}
/**
* @brief The 'regression2D' method can be used to fit a line to a given point set.
* @param points_begin set begin iterator
* @param points_end set end iterator
* @param fit_start the start of the line fit
* @param fit_end the set termintating iterator position
* @param line the parameterized line to work with
*/
inline void regression2D(const std::vector<LaserBeam>::const_iterator &points_begin, const std::vector<LaserBeam>::const_iterator &points_end,
Eigen::ParametrizedLine<double, 2> &line)
{
std::vector<LaserBeam>::const_iterator back_it = points_end;
--back_it;
Eigen::Vector2d front (points_begin->posX(), points_end->posY());
Eigen::Vector2d back (back_it->posX(), back_it->posY());
unsigned int size = std::distance(points_begin, points_end);
Eigen::MatrixXd A(size, 2);
Eigen::VectorXd b(size);
A.setOnes();
Eigen::Vector2d dxy = (front - back).cwiseAbs();
bool solve_for_x = dxy.x() > dxy.y();
if(solve_for_x) {
/// SOLVE FOR X
int i = 0;
for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
{
A(i,1) = it->posX();
b(i) = it->posY();
}
} else {
/// SOLVE FOR Y
int i = 0;
for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
{
A(i,1) = it->posY();
b(i) = it->posX();
}
}
Eigen::VectorXd weights = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
double alpha = weights(0);
double beta = weights(1);
Eigen::Vector2d from;
Eigen::Vector2d to;
if(solve_for_x) {
from(0) = 0.0;
from(1) = alpha;
to(0) = 1.0;
to(1) = alpha + beta;
} else {
from(0) = alpha;
from(1) = 0.0;
to(0) = alpha + beta;
to(1) = 1.0;
}
Eigen::Vector2d fit_start;
Eigen::Vector2d fit_end;
line = Eigen::ParametrizedLine<double, 2>(from, (to - from).normalized());
fit_start = line.projection(front);
fit_end = line.projection(back);
line = Eigen::ParametrizedLine<double, 2>(fit_start, (fit_end - fit_start));
}
void ElevationVisualization::visualize(
const grid_map::GridMap& map,
const std::string& typeNameElevation,
const std::string& typeNameColor,
const float lowerValueBound,
const float upperValueBound)
{
// Set marker info.
marker_.header.frame_id = map.getFrameId();
marker_.header.stamp.fromNSec(map.getTimestamp());
marker_.scale.x = map.getResolution();
marker_.scale.y = map.getResolution();
// Clear points.
marker_.points.clear();
marker_.colors.clear();
float markerHeightOffset = static_cast<float>(markerHeight_/2.0);
const Eigen::Array2i buffSize = map.getBufferSize();
const Eigen::Array2i buffStartIndex = map.getBufferStartIndex();
const bool haveColor = map.isType("color");
for (unsigned int i = 0; i < buffSize(0); ++i)
{
for (unsigned int j = 0; j < buffSize(1); ++j)
{
// Getting map data.
const Eigen::Array2i cellIndex(i, j);
const Eigen::Array2i buffIndex =
grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex);
const float& elevation = map.at(typeNameElevation, buffIndex);
if(std::isnan(elevation))
continue;
const float color = haveColor ? map.at(typeNameColor, buffIndex) : lowerValueBound;
// Add marker point.
Eigen::Vector2d position;
map.getPosition(buffIndex, position);
geometry_msgs::Point point;
point.x = position.x();
point.y = position.y();
point.z = elevation - markerHeightOffset;
marker_.points.push_back(point);
// Add marker color.
if(haveColor)
{
std_msgs::ColorRGBA markerColor =
grid_map_visualization::color_utils::interpolateBetweenColors(
color, lowerValueBound, upperValueBound, lowerColor_, upperColor_);
marker_.colors.push_back(markerColor);
}
}
}
markerPublisher_.publish(marker_);
}
void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Eigen::Array2i& startIndex, Eigen::Array2i& bufferSize) const
{
Eigen::Vector2d topLeft = polygon_.getVertices()[0];
Eigen::Vector2d bottomRight = topLeft;
for (const auto& vertex : polygon_.getVertices()) {
topLeft = topLeft.array().max(vertex.array());
bottomRight = bottomRight.array().min(vertex.array());
}
limitPositionToRange(topLeft, mapLength_, mapPosition_);
limitPositionToRange(bottomRight, mapLength_, mapPosition_);
getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
Eigen::Array2i endIndex;
getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
bufferSize = endIndex - startIndex + Eigen::Array2i::Ones();
}
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;
}
void Camera::pan(Eigen::Vector2i oldPosition, Eigen::Vector2i newPosition){
const Eigen::Vector2d delta =
(newPosition - oldPosition).eval().template cast<double>();
const Eigen::Vector3d forwardVector = lookAt - position;
const Eigen::Vector3d rightVector = forwardVector.cross(up);
const Eigen::Vector3d upVector = rightVector.cross(forwardVector);
const double scale = 0.0005;
position += scale*(-delta.x()*rightVector +
delta.y()*upVector);
}
/**
* @brief Retrieve the cartesian point coordinates of a range reading.
* @param reading the range reading
* @param angular_res the angular resolution of the range reading
* @param min_angle the minimum angle of the range reading
* @param points the point coordiantes in cartesian space
*/
inline void polarToCartesian(const std::vector<float> &reading, const float angular_res, const float min_angle,
const float min_rho, const float max_rho, std::vector<LaserBeam> &points)
{
points.clear();
double angle = min_angle;
for(std::vector<float>::const_iterator it = reading.begin() ; it != reading.end() ; ++it, angle += angular_res) {
float rho = *it;
if(rho < max_rho && rho > min_rho && rho > 0.0) {
Eigen::Vector2d pt;
pt.x() = std::cos(angle) * rho;
pt.y() = std::sin(angle) * rho;
points.push_back(pt);
}
}
}
void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
Index& startIndex, Size& bufferSize) const
{
const Eigen::Rotation2Dd rotationMatrix(rotation);
Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
Position topLeft = center.array() + boundingBoxHalfLength;
Position bottomRight = center.array() - boundingBoxHalfLength;
limitPositionToRange(topLeft, mapLength_, mapPosition_);
limitPositionToRange(bottomRight, mapLength_, mapPosition_);
getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
Index endIndex;
getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
bufferSize = endIndex - startIndex + Eigen::Array2i::Ones();
}
/* This callback is called everytime this node's simulation starts or restarts.
This is different from initialize() above. */
virtual int64_t onInitialization() override {
// Initial state and output
x.setZero();
velocity = 0.0;
std::cout << "At " << currentSimulationTime() << " INIT" << std::endl;
return 0;
}
void VertexCell::drawPickCustom(Time time, ViewSettings & /*viewSettings*/)
{
if(!exists(time))
return;
int n = 50;
Eigen::Vector2d p = pos(time);
glBegin(GL_POLYGON);
{
double r = 0.5 * size(time);
for(int i=0; i<n; ++i)
{
double theta = 2 * (double) i * 3.14159 / (double) n ;
glVertex2d(p.x() + r*std::cos(theta),p.y()+ r*std::sin(theta));
}
}
glEnd();
}
void DepthObstacleGrid::initializeStatics(NodeMap *map){
Environment env = map->getEnvironment();
for(std::vector<Plane>::iterator it = env.planes.begin(); it != env.planes.end(); it++){
double plane_length = Eigen::Vector2d( it->span_horizontal.x(), it->span_horizontal.y() ).norm();
Eigen::Vector2d step = Eigen::Vector2d( it->span_horizontal.x(), it->span_horizontal.y() ) / (plane_length / (0.5 * resolution) );
double step_length = step.norm();
Eigen::Vector2d lastCell(NAN, NAN);
Eigen::Vector2d pos = Eigen::Vector2d(it->position.x(), it->position.y());
//iterate through the cells
for(double i = 0.0; i < plane_length; i += step_length){
Eigen::Vector2d cell_coord = getGridCoord( pos.x(), pos.y() );
//step was not wide enough, we are still in the same cell
if(cell_coord != lastCell){
lastCell = cell_coord;
Eigen::Vector2i ID = getCellID(cell_coord.x(), cell_coord.y());
//Step was invalid, we are outside the grid
if(ID.x() == -1){
pos += step;
continue;
}
//Set the cell static
GridElement &elem = get(ID.x(), ID.y());
elem.static_obstacle = true;
}
pos += step;
}
}
}
Eigen::Vector2d snell_s (double n1, double n2, const Eigen::Vector2d &input_normal_vector, const Eigen::Vector2d &input_incident_vector)
{
Eigen::Vector2d normal_vector = input_normal_vector.normalized();
Eigen::Vector2d incident_vector = input_incident_vector.normalized();
if (n1 > n2) //check for critcal angle
{
double critical_angle = asin(n1/n2);
double angle_between_n_I = acos(normal_vector.dot(incident_vector)/(normal_vector.norm()*incident_vector.norm()));
if (angle_between_n_I>critical_angle)
{
throw 20;
}
}
double c = normal_vector.dot(incident_vector);
double r = n1/n2;
Eigen::Vector2d v_refract = (r*c - sqrt(1-r*r*(1-c*c)))*normal_vector - r*incident_vector;
return v_refract;
}
/**
In this formulation of the Multi-Dimensional Newton-Raphson solver the Jacobian matrix is known.
Therefore, the dx vector can be obtained from
J(x)dx=-f(x)
for a given value of x. The pointer to the class FuncWrapperND that is passed in must implement the call() and Jacobian()
functions, each of which take the vector x. The data is managed using std::vector<double> vectors
@param f A pointer to an subclass of the FuncWrapperND class that implements the call() and Jacobian() functions
@param x0 The initial guess value for the solution
@param tol The root-sum-square of the errors from each of the components
@param maxiter The maximum number of iterations
@param errstring A string with the returned error. If the length of errstring is zero, no errors were found
@returns If no errors are found, the solution. Otherwise, _HUGE, the value for infinity
*/
std::vector<double> NDNewtonRaphson_Jacobian(FuncWrapperND *f, std::vector<double> &x0, double tol, int maxiter)
{
int iter=0;
f->errstring.clear();
std::vector<double> f0,v;
std::vector<std::vector<double> > JJ;
Eigen::VectorXd r(x0.size());
Eigen::Matrix2d J(x0.size(), x0.size());
double error = 999;
while (iter==0 || std::abs(error)>tol){
f0 = f->call(x0);
JJ = f->Jacobian(x0);
for (std::size_t i = 0; i < x0.size(); ++i)
{
r(i) = f0[i];
for (std::size_t j = 0; j < x0.size(); ++j)
{
J(i,j) = JJ[i][j];
}
}
Eigen::Vector2d v = J.colPivHouseholderQr().solve(-r);
// Update the guess
for (std::size_t i = 0; i<x0.size(); i++){ x0[i] += v(i);}
// Stop if the solution is not changing by more than numerical precision
if (v.cwiseAbs().maxCoeff() < DBL_EPSILON*100){
return x0;
}
error = root_sum_square(f0);
if (iter>maxiter){
f->errstring = "reached maximum number of iterations";
x0[0] = _HUGE;
}
iter++;
}
return x0;
}
double BlockConstraint::_basicCostGrad(Eigen::Vector2d& grad_c, const Vector2d &config)
{
Eigen::Vector2d p = _tfinv*(config-_center);
// std::cout << p.transpose() << "\t|\t" << scales[0] << ", " << scales[1] << std::endl;
assert( buffer > 0 && "We only support positive-valued buffer sizes!");
double c = INFINITY;
for(size_t i=0; i<2; ++i)
{
if(fabs(p[i]) >= fabs(scales[i])+fabs(buffer))
{
grad_c.setZero();
return 0;
}
else if(fabs(p[i]) >= fabs(scales[i]))
{
double dist = buffer-(fabs(p[i])-fabs(scales[i]));
double ctemp = dist*dist/(2*buffer);
if(ctemp < c)
{
c = ctemp;
grad_c.setZero();
grad_c[i] = -(fabs(p[i])-fabs(scales[i])-buffer)/buffer*sign(p[i]);
}
}
else
{
double ctemp = scales[i]-p[i] + buffer/2;
if(ctemp < c)
{
c = ctemp;
grad_c.setZero();
grad_c[i] = -sign(p[i]);
}
}
}
return c;
}
/**
Eigen::Vector2d internal_ray_lens_intersection( const Eigen::Vector2d &direction_of_internal, const Eigen::Vector2d &origin_of_internal, const Eigen::Vector2d ¢erpoint_of_sphere, double radius_of_sphere)
{
//https://people.cs.clemson.edu/~dhouse/courses/405/notes/raycast.pdf
Eigen::Vector2d return_intersection;
Eigen::Vector2d direction_of_internal_normalized = direction_of_internal.normalized();
Eigen::Vector2d o_minus_c = origin_of_internal-centerpoint_of_sphere;
double discriminant = (direction_of_internal_normalized.dot(o_minus_c))*(direction_of_internal_normalized.dot(o_minus_c))-o_minus_c.norm()*o_minus_c.norm()+radius_of_sphere*radius_of_sphere;
if (discriminant < 0)
{
throw 30;
}
else
{
double d_1 = -1*(direction_of_internal_normalized.dot(o_minus_c))+sqrt((direction_of_internal_normalized.dot(o_minus_c))*(direction_of_internal_normalized.dot(o_minus_c))-o_minus_c.norm()*o_minus_c.norm()+radius_of_sphere*radius_of_sphere);
double d_2 = -1*(direction_of_internal_normalized.dot(o_minus_c))-sqrt((direction_of_internal_normalized.dot(o_minus_c))*(direction_of_internal_normalized.dot(o_minus_c))-o_minus_c.norm()*o_minus_c.norm()+radius_of_sphere*radius_of_sphere);
if (d_2=d_1)
{
return_intersection = origin_of_internal+d_2*direction_of_internal_normalized;
}
else
{
Eigen::Vector2d return_intersection_1 = origin_of_internal+d_1*direction_of_internal_normalized;
Eigen::Vector2d return_intersection_2 = origin_of_internal+d_2*direction_of_internal_normalized;
if (return_intersection_1.y()<return_intersection_2.y())
{
return_intersection = return_intersection_1;
}
else
{
return_intersection = return_intersection_2;
}
}
cout << "lens choice 1: " << d_1 << endl;
cout << "lens choice 2: " << d_2 << endl;
}
return return_intersection;
}
**/
Eigen::Vector2d internal_ray_lens_intersection( const Eigen::Vector2d &direction_of_internal, const Eigen::Vector2d &origin_of_internal, const Eigen::Vector2d ¢erpoint_of_sphere, double radius_of_sphere)
{
// https://people.cs.clemson.edu/~dhouse/courses/405/notes/raycast.pdf
Eigen::Vector2d return_intersection;
Eigen::Vector2d direction_of_internal_normalized = direction_of_internal.normalized();
Eigen::Vector2d o_minus_c = origin_of_internal-centerpoint_of_sphere;
double t_close = direction_of_internal.dot(centerpoint_of_sphere-origin_of_internal);
Eigen::Vector2d x_close = origin_of_internal + t_close*direction_of_internal;
double discriminant = (x_close-centerpoint_of_sphere).norm();
if (discriminant < radius_of_sphere)
{
double a = sqrt(radius_of_sphere*radius_of_sphere-discriminant*discriminant);
return_intersection = origin_of_internal + (t_close-a)*direction_of_internal;
//cout << "Centerpoint of sphere : " << centerpoint_of_sphere << endl;
//cout << "radius of sphere : " << radius_of_sphere << endl;
}
else
{
throw 30;
}
return return_intersection;
}
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;
}
const Eigen::Vector2d CMiniVisionToolbox::getPointDistortedPlumbBob( const Eigen::Vector2d& p_vecPointUndistorted, const Eigen::Vector2d& p_vecPointPrincipal, const Eigen::Vector4d& p_vecDistortionCoefficients )
{
const int32_t iUndistortedX( std::abs( p_vecPointUndistorted.x( ) ) );
const int32_t iUndistortedY( std::abs( p_vecPointUndistorted.y( ) ) );
//ds current ranges
const int32_t iXStart( iUndistortedX-40 );
const int32_t iXStop( iUndistortedX+40 );
const int32_t iYStart( iUndistortedY-40 );
const int32_t iYStop( iUndistortedY+40 );
double dErrorLowest( 10.0 );
Eigen::Vector2d vecPointDistorted( 0, 0 );
//ds loop around the current point
for( int32_t i = iXStart; i < iXStop; ++i )
{
for( int32_t j = iYStart; j < iYStop; ++j )
{
/*ds get current error
const Eigen::Vector2d vecError( p_vecPointUndistorted-CMiniVisionToolbox::getPointUndistortedPlumbBob( Eigen::Vector2i( iUndistortedX, iUndistortedY ), p_vecPointPrincipal, p_vecDistortionCoefficients ) );
//ds error
const double dErrorCurrent( std::fabs( vecError(0) ) + std::fabs( vecError(1) ) );
//ds if the error is lower
if( dErrorLowest > dErrorCurrent )
{
vecPointDistorted = Eigen::Vector2d( iUndistortedX, iUndistortedY );
}*/
}
}
std::printf( "error: %f\n", dErrorLowest );
return vecPointDistorted;
}
void analytical_derivative(const Camera& camera_, const Point3d& point_, const Eigen::Vector2d& obs, Eigen::Matrix<double,2,12>& j)
{
const T camera[3] = {T(camera_[0],0),T(camera_[1],1),T(camera_[2],2)};
const T point[3] = {T(point_[0],9),T(point_[1],10),T(point_[2],11)};
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
p[0] += T(camera_[3],3);
p[1] += T(camera_[4],4);
p[2] += T(camera_[5],5);
auto xp = - p[0] / p[2];
auto yp = - p[1] / p[2];
const T l1(camera_[7],7);
const T l2(camera_[8],8);
auto r2 = xp*xp + yp*yp;
auto distortion = T(1.0) + r2 * (l1 + l2 * r2);
T focal(camera_[6],6);
auto jacobx = focal * distortion * xp - T(obs.x());
auto jacoby = focal * distortion * yp - T(obs.y());
for(int i = 0 ; i < 12 ; ++i)
{
j(0,i) = jacobx.v[i];
j(1,i) = jacoby.v[i];
}
}
Eigen::MatrixXd sqExp2d(const Eigen::Matrix<double, 2, -1> &x1,
const Eigen::Matrix<double, 2, -1> &x2,
const Eigen::Vector2d &lengthScale, bool noisy)
{
// assert(x1.rows() == x2.rows())
int n1 = x1.cols(), n2 = x2.cols();
// Compute the weighted square distances
Eigen::Vector2d w = (lengthScale.array().square().cwiseInverse()).matrix();
Eigen::RowVectorXd xx1 = w.replicate(1, n1).cwiseProduct(x1).cwiseProduct(x1).colwise().sum();
Eigen::RowVectorXd xx2 = w.replicate(1, n2).cwiseProduct(x2).cwiseProduct(x2).colwise().sum();
Eigen::MatrixXd x1x2 = w.replicate(1, n1).cwiseProduct(x1).transpose() * x2;
// Compute the covariance matrix
Eigen::MatrixXd K = (-0.5 *
Eigen::MatrixXd::Zero(n1, n2).cwiseMax(
xx1.transpose().replicate(1, n2) + xx2.replicate(n1, 1) - 2 * x1x2)).array().exp();
if (noisy) {
K += K.colwise().sum().asDiagonal();
}
return K;
}
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 GeometryUtils::angleLike(const Eigen::Vector2d & u, const Eigen::Vector2d & v)
{
// assume u and v are unitary
/*
double lu = length(u);
if(lu>0) u = 1/lu * u;
double lv=length(v);
if(lv>0) v = 1/lv * v;
*/
double dot_ = u.dot(v);
double det_ = u[0]*v[1] - u[1]*v[0];
double signDet = 1;
if(det_ < 0) signDet = -1;
return 2 - signDet*(dot_+1);
}