Vector4d Matrix4d::operator* (Vector4d& other){ Vector4d result; for (int i = 0; i < 4; i++){ result.set(i, other[0] * m[0][i] + other[1] * m[1][i] + other[2] * m[2][i] + other[3] * m[3][i]); } return result; }
ColorBank::Colorf ColorBank::colorf(DotPath const &path) const { if (path.isEmpty()) return Colorf(); Vector4d clamped = data(path).as<Impl::ColorData>().color; clamped = clamped.max(Vector4d(0, 0, 0, 0)).min(Vector4d(1, 1, 1, 1)); return Colorf(float(clamped.x), float(clamped.y), float(clamped.z), float(clamped.w)); }
Vector PickObjectTool::GetWorldCoordinates(BaseDraw* bd, const Matrix4d& m, Float x, Float y, Float z) { // pick object returns the view-projection matrix. This transforms a point in camera space into clip space. Int32 l, t, r, b, w, h; Vector4d pos; Vector posWorld; bd->GetFrame(&l, &t, &r, &b); if (l == r || b == t) return Vector(0.0); w = r - l; h = b - t; // first, transform the points into clip space pos.x = (x - Float(l)) / Float(w); pos.y = (y - Float(t)) / Float(h); pos.z = z; pos.w = 1.0; pos = pos * 2.0f - Vector4d(1.0f); pos.y = -pos.y; // apply the inverse view transform Matrix4d im = !m; pos = im * pos; pos.MakeVector3(); // convert it into a 3-tupel posWorld = bd->GetMg() * GetVector3(pos); return posWorld; }
/** * operating with 3-vectors here because there are a lot of cross products */ double Triangle::intersection(const ray &viewRay)const { Vector4d edge1 = p2 - p1; Vector4d edge2 = p3 - p1; Vector4d h = cross(viewRay.dir, edge2); double a = edge1.dot(h); if (a > -EPSILON && a < EPSILON) return -1 * std::numeric_limits<double>::max(); double f = 1/a; Vector4d s = viewRay.orig - p1; double u = f * s.dot(h); if (u < 0.0 || u > 1.0) return -1 * std::numeric_limits<double>::max(); Vector4d q = cross(s, edge1); double v = f * q.dot(viewRay.dir); if (v < 0.0 || u + v > 1.0) return -1 * std::numeric_limits<double>::max(); double t = f * edge2.dot(q); return t; }
int addPointAndProjection(SysSBA& sba, vector<Point, Eigen::aligned_allocator<Point> >& points, int ndi) { // Define dimensions of the image. int maxx = 640; int maxy = 480; // Project points into nodes. for (int i = 0; i < points.size(); i++) { double pointnoise = 0.1; // Add points into the system, and add noise. // Add up to .5 pixels of noise. Vector4d temppoint = points[i]; temppoint.x() += pointnoise*(drand48() - 0.5); temppoint.y() += pointnoise*(drand48() - 0.5); temppoint.z() += pointnoise*(drand48() - 0.5); int index = sba.addPoint(temppoint); Vector3d proj; calculateProj(sba, points[i], ndi, proj); // If valid (within the range of the image size), add the stereo // projection to SBA. //if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy) //{ sba.addStereoProj(ndi, index, proj); //} } return sba.tracks.size() - points.size(); }
Vector6d gc_pipi_plk( Vector4d pi1, Vector4d pi2){ Vector6d plk; Matrix4d dp = pi1 * pi2.transpose() - pi2 * pi1.transpose(); plk << dp(0,3), dp(1,3), dp(2,3), - dp(1,2), dp(0,2), - dp(0,1); return plk; }
void rigidBodyConstraintParseQuat(const mxArray* pm, Vector4d &quat) { if(!mxIsNumeric(pm)) { mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be a 4x1 double vector"); } if(!(mxGetM(pm) == 4 && mxGetN(pm) == 1)) { mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be of size 4x1"); } memcpy(quat.data(),mxGetPr(pm),sizeof(double)*4); for(int i = 0;i<4;i++) { if((mxIsInf(quat(i)))||(mxIsNaN(quat(i)))) { mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 cannot have entry equal to NaN or Inf"); } } double quat_norm = quat.norm(); if(quat_norm==0.0) { mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The Input argument 1 must be a nonzero vector"); } quat = quat/quat_norm; }
Vector4d Triangle::normal_vector(const Vector4d &surface) const { double u, v; std::tie(u, v) = uvCoords(surface); Vector4d normal = (1 - u - v) * n1 + u * n2 + v * n3; normal.normalize(); return normal; }
void addnode(SysSPA &spa, int n, // node translation std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans, // node rotation std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot, // constraint indices std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind, // constraint local translation std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans, // constraint local rotation as quaternion std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot, // constraint covariance std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > cvar) { Node nd; // rotation Quaternion<double> frq; frq.coeffs() = nqrot[n]; frq.normalize(); if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs(); nd.qrot = frq.coeffs(); // translation Vector4d v; v.head(3) = ntrans[n]; v(3) = 1.0; nd.trans = v; nd.setTransform(); // set up world2node transform nd.setDr(true); // add to system spa.nodes.push_back(nd); // add in constraints for (int i=0; i<(int)ctrans.size(); ++i) { ConP2 con; con.ndr = cind[i].x(); con.nd1 = cind[i].y(); if ((con.ndr == n && con.nd1 <= n-1) || (con.nd1 == n && con.ndr <= n-1)) { con.tmean = ctrans[i]; Quaternion<double> qr; qr.coeffs() = cqrot[i]; qr.normalize(); con.qpmean = qr.inverse(); // inverse of the rotation measurement con.prec = cvar[i]; // ??? should this be inverted ??? // need a boost for noise-offset system //con.prec.block<3,3>(3,3) *= 10.0; spa.p2cons.push_back(con); } } }
void Model::RotateObject(Shape* shape, TreeObject* object, Vector4d rotate) { if (!shape) return; Vector3d rot(rotate.x(), rotate.y(), rotate.z()); shape->Rotate(rot, rotate.w()); ModelChanged(); }
Vector3d transformPoint(Vector3d& point,const MatrixXd& transform) { Vector4d tmp; tmp.head(3)=point; tmp(3)=1; tmp.head(3)=transform*tmp; return tmp.head(3); }
void CSimuVertexRingObj::_computeElasticForcesNew( const unsigned int timeid, const bool isStatic, const bool needjacobian) { //compute the rotation for each vertex const bool bRecompRot = (timeid%2 == 1); if (timeid<=1 || bRecompRot){ const bool needquat = false; updateRotationQuaternionForAllElements(timeid, needquat); } else{ //use central difference to update the quaternions for (int i=0; i<m_nVRingElementCount; i++){ CVertexRingElement &e = m_pVRingElement[i]; Vector4d quat = e.m_quat + e.m_quat - e.m_quat0; quat.normalize(); e.m_quat0 = e.m_quat; e.m_quat = quat; //convert quat to matrix Quaternion *pquat = (Quaternion*)&e.m_quat.x; typedef double M33[3][3]; pquat->getRotationMatrix(*((M33*)&e.m_R.x)); } } //call the old alg. const int SKIPSTEP = m_nRotationSkipStep; m_nRotationSkipStep = INT_MAX-1; _computeElasticForces(timeid, isStatic, needjacobian); m_nRotationSkipStep = SKIPSTEP; /* //compute forces using the rotation Vector3d force; double3x3 jacobian, *pjac=NULL; const bool FASTSTIFF = (this->m_mtl.getMaterialType()==0); if (needjacobian) pjac = &jacobian; for (int i=0; i<m_nEdge; i++){ CSimuEdgeInput& edge = m_pEdge[i]; const int v0=edge.v0, v1=edge.v1; if (bRecompRot){ //computer averaged rotation of the rod, then save it const Quaternion *q0 = (const Quaternion *)(&m_pVRingElement[v0].m_quat); const Quaternion *q1 = (const Quaternion *)(&m_pVRingElement[v1].m_quat); const Quaternion q = Quaternion::slerp_midpoint(*q0, *q1); q.getRotationMatrix(*((double(*)[3][3])(&edge.mat))); } //apply the rotation for the rod element const Vector3d& p0 = m_pVertInfo[v0].m_pos; const Vector3d& p1 = m_pVertInfo[v1].m_pos; m_pGyrod[i].computeNodalForce(p0, p1, edge.mat, *edge.pMaterial, force, pjac); //accumulate the force and stiffness m_pVertInfo[v0].m_force+=force; m_pVertInfo[v1].m_force-=force; if (needjacobian) saveVertSitffMatrixIntoSparseMatrix(v0, v1, *pjac, FASTSTIFF); } */ }
inline bool cullSphere(const Vector4d& center, double radius) const { return (center.z() - radius > m_nearZ || center.z() + radius < m_farZ || center.dot(m_planeNormals[0]) < -radius || center.dot(m_planeNormals[1]) < -radius || center.dot(m_planeNormals[2]) < -radius || center.dot(m_planeNormals[3]) < -radius); }
Vector4d rigidBodyDynamics::mrp2quaternion(Vector3d mrp) const{ Vector4d dq; dq << 8*mrp / (16 + mrp.transpose() * mrp), (16 - mrp.transpose() * mrp) / (16+mrp.transpose() * mrp); // std::cout << "MRP: " << mrp.transpose() << std::endl; // std::cout << "dq_prenorm: " << dq.transpose() << std::endl; dq /=dq.norm(); // std::cout << "dq_postnorm: " << dq.transpose() << std::endl; return dq; }
double geometric(Vector4d s1, Vector4d n1, Vector4d s2, Vector4d n2) { ASSERT(isUnitVector(n1), "assumes the normals are normalized"); ASSERT(isUnitVector(n2), "assumes the normals are normalized"); Vector4d segment = s1 - s2; Vector4d seg_dir = segment.normalized(); double cos1 = n1.dot(seg_dir); double cos2 = n2.dot(-1 * seg_dir); return std::abs(cos1 * cos2) / segment.squaredNorm(); }
// Project a 3D point into this Pose. Vector2d Pose::ProjectTo2D(const Vector3d& pt3d) { Vector4d pt3d_h = Vector4d::Constant(1.0); pt3d_h.head(3) = pt3d; const Vector4d proj_h = Rt_ * pt3d_h; Vector2d proj = proj_h.head(2); proj /= proj_h(2); return proj; }
Vector4d CProxyCamera::intersect(const Vector4d &coord, const Vector4d& abcd) const { Vector4d ray = m_center - coord; const double A = coord.dot(abcd); const double B = ray.dot(abcd); if (fabs(B)<1e-8) return Vector4d(0.0f, 0.0f, 0.0f, -1.0f); else return coord - A / B * ray; }
// Test a point to see if it lies within the frustum defined by // planes z=nearZ, z=farZ, and the four side planes with specified // normals. static inline bool frustumCull(const Vector4d& curvePoint, double curveBoundingRadius, double nearZ, double farZ, const Vector4d viewFrustumPlaneNormals[]) { return (curvePoint.z() - curveBoundingRadius > nearZ || curvePoint.z() + curveBoundingRadius < farZ || curvePoint.dot(viewFrustumPlaneNormals[0]) < -curveBoundingRadius || curvePoint.dot(viewFrustumPlaneNormals[1]) < -curveBoundingRadius || curvePoint.dot(viewFrustumPlaneNormals[2]) < -curveBoundingRadius || curvePoint.dot(viewFrustumPlaneNormals[3]) < -curveBoundingRadius); }
/** Add a new sample to the path. If the sample time is less than the first time, * it is added at the end. If it is greater than the last time, it is appended * to the path. The sample is ignored if it has a time in between the first and * last times of the path. */ void CurvePlot::addSample(const CurvePlotSample& sample) { bool addToBack = false; if (m_samples.empty() || sample.t > m_samples.back().t) { addToBack = true; } else if (sample.t < m_samples.front().t) { addToBack = false; } else { // Sample falls within range of current samples; discard it return; } if (addToBack) m_samples.push_back(sample); else m_samples.push_front(sample); if (m_samples.size() > 1) { // Calculate a bounding radius for this segment. No point on the curve will // be further from the start point than the bounding radius. if (addToBack) { const CurvePlotSample& lastSample = m_samples[m_samples.size() - 2]; double dt = sample.t - lastSample.t; Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(lastSample.position), zeroExtend(sample.position), zeroExtend(lastSample.velocity * dt), zeroExtend(sample.velocity * dt)); Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0); m_samples[m_samples.size() - 1].boundingRadius = extents.norm(); } else { const CurvePlotSample& nextSample = m_samples[1]; double dt = nextSample.t - sample.t; Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(sample.position), zeroExtend(nextSample.position), zeroExtend(sample.velocity * dt), zeroExtend(nextSample.velocity * dt)); Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0); m_samples[1].boundingRadius = extents.norm(); } } }
Vector4d rigidBodyDynamics::quaternionFromRot(Matrix3d& R) const{ Vector4d q; double div1, div2, div3, div4; double numerical_limit = 1.0e-4; if (abs(R.determinant()-1) > numerical_limit ) { std::cerr << "R does not have a determinant of +1" << std::endl; } else { div1 = 0.5*sqrt(1+R(0,0)+R(1,1)+R(2,2)); div2 = 0.5*sqrt(1+R(0,0)-R(1,1)-R(2,2)); div3 = 0.5*sqrt(1-R(0,0)-R(1,1)+R(2,2)); div4 = 0.5*sqrt(1-R(0,0)+R(1,1)-R(2,2)); //if (div1 > div2 && div1 > div3 && div1 > div4) { if (fabs(div1) > numerical_limit) { q(3) = div1; q(0) = 0.25*(R(1,2)-R(2,1))/q(3); q(1) = 0.25*(R(2,0)-R(0,2))/q(3); q(2) = 0.25*(R(0,1)-R(1,0))/q(3); } else if (fabs(div2) > numerical_limit) { //} else if (div2 > div1 && div2 > div3 && div2 > div4) { q(0) = div2; q(1) = 0.25*(R(0,1)+R(1,0))/q(0); q(2) = 0.25*(R(0,2)+R(2,0))/q(0); q(3) = 0.25*(R(1,2)+R(2,1))/q(0); } else if (fabs(div3) > numerical_limit) { //} else if (div3 > div1 && div3 > div2 && div3 > div4) { q(2) = div3; q(0) = 0.25*(R(0,2)+R(2,0))/q(2); q(1) = 0.25*(R(1,2)+R(2,1))/q(2); q(3) = 0.25*(R(0,1)-R(1,0))/q(2); //} else { } else if (fabs(div4) > numerical_limit) { q(1) = div4; q(0) = 0.25*(R(0,1)+R(1,0))/q(1); q(2) = 0.25*(R(1,2)+R(2,1))/q(1); q(3) = 0.25*(R(2,0)-R(0,2))/q(1); } else { std::cerr << "quaternionFromRot didn't convert: [" << div1 << ", " << div2 << ", " << div3 << ", " << div4 << std::endl; std::cerr << "Rotation Matrix: " << R << std::endl; } } /* if (q(3) < 0) { q *= -1; } */ q /=q.norm(); return q; }
Vector4d rigidBodyDynamics::quaternionMultiplication(Vector4d& q1, Vector4d& q2) const { //q1 \mult q2 Matrix4d qm; Vector4d result; qm << q1(3), q1(2), -q1(1), q1(0), -q1(2), q1(3), q1(0), q1(1), q1(1), -q1(0), q1(3), q1(2), -q1(0), -q1(1), -q1(2), q1(3); result = qm*q2; result /= result.norm(); return result; }
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if(nrhs != 5) { mexErrMsgIdAndTxt("Drake:testQuatmex:BadInputs","Usage [r,dr,e,ed,quat,dquat,q3,dq3,w,dw] = testQuatmex(q1,q2,axis,u,v)"); } Vector4d q1; Vector4d q2; memcpy(q1.data(),mxGetPr(prhs[0]),sizeof(double)*4); memcpy(q2.data(),mxGetPr(prhs[1]),sizeof(double)*4); Vector4d r = quatDiff(q1,q2); Matrix<double,4,8> dr = dquatDiff(q1,q2); plhs[0] = mxCreateDoubleMatrix(4,1,mxREAL); plhs[1] = mxCreateDoubleMatrix(4,8,mxREAL); memcpy(mxGetPr(plhs[0]),r.data(),sizeof(double)*4); memcpy(mxGetPr(plhs[1]),dr.data(),sizeof(double)*4*8); Vector3d axis; memcpy(axis.data(),mxGetPr(prhs[2]),sizeof(double)*3); double e = quatDiffAxisInvar(q1,q2,axis); Matrix<double,1,11> de = dquatDiffAxisInvar(q1,q2,axis); plhs[2] = mxCreateDoubleScalar(e); plhs[3] = mxCreateDoubleMatrix(1,11,mxREAL); memcpy(mxGetPr(plhs[3]),de.data(),sizeof(double)*11); Vector3d u,v; Vector4d quat; Matrix<double,4,6> dquat; memcpy(u.data(),mxGetPr(prhs[3]),sizeof(double)*3); memcpy(v.data(),mxGetPr(prhs[4]),sizeof(double)*3); Vector4d q3 = quatProduct(q1,q2); Matrix<double,4,8> dq3 = dquatProduct(q1,q2); plhs[4] = mxCreateDoubleMatrix(4,1,mxREAL); plhs[5] = mxCreateDoubleMatrix(4,8,mxREAL); memcpy(mxGetPr(plhs[4]),q3.data(),sizeof(double)*4); memcpy(mxGetPr(plhs[5]),dq3.data(),sizeof(double)*4*8); Vector3d w = quatRotateVec(q1,u); Matrix<double,3,7> dw = dquatRotateVec(q1,u); plhs[6] = mxCreateDoubleMatrix(3,1,mxREAL); plhs[7] = mxCreateDoubleMatrix(3,7,mxREAL); memcpy(mxGetPr(plhs[6]),w.data(),sizeof(double)*3); memcpy(mxGetPr(plhs[7]),dw.data(),sizeof(double)*3*7); }
Eigen::Matrix<typename Derived::Scalar, 4, 1> rpy2quat(const Eigen::MatrixBase<Derived>& rpy) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3); auto rpy_2 = (rpy / 2.0).array(); auto s = rpy_2.sin(); auto c = rpy_2.cos(); Vector4d q; q << c(0)*c(1)*c(2) + s(0)*s(1)*s(2), s(0)*c(1)*c(2) - c(0)*s(1)*s(2), c(0)*s(1)*c(2) + s(0)*c(1)*s(2), c(0)*c(1)*s(2) - s(0)*s(1)*c(2); q /= q.norm() + std::numeric_limits<typename Derived::Scalar>::epsilon(); return q; }
Vector6d gc_aid_to_av(Vector4d aid) { Vector6d av; Vector3d aa = aid.head(3); double d = 1.0 / aid(3); Matrix3d R = gc_Rodriguez(aa); av.head(3) = R.col(2) * d; av.tail(3) = R.col(0); // Vector6d av; // double a = aid[0]; // double b = aid[1]; // double g = aid[2]; // double t = aid[3]; // // double s1 = sin(a); // double c1 = cos(a); // double s2 = sin(b); // double c2 = cos(b); // double s3 = sin(g); // double c3 = cos(g); // // Matrix3d R; // R << // c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3, // c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3, // -s2, s1 * c2, c1 * c2; // // double d = 1.0 / t; // av.head(3) = -R.col(2) * d; // av.tail(3) = R.col(1); return av; }
Vector6d gc_asd_to_av(Vector4d asd) { Vector6d av; Vector3d aa = asd.head(3); // double d_inv = asd(3); // double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3)))); // double sig_d_inv = -log(1.0/asd(3) - 1.0); // double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) ); // double sig_d_inv = atan(asd(3)) / 2.0; // double sig_d_inv = atan2(asd(3), 1.0) / 2.0; // double sig_d_inv = atan2(asd(3), 1.0); // double sig_d_inv = atan2(asd(3), 1.0) * 1.0; // double sig_d_inv = tan(4.0 * asd(3)); double sig_d_inv = log(asd(3)); // cout << "sig_d_inv = " << sig_d_inv << endl; // double sig_d_inv = cos(asd(3)) / sin(asd(3)); // double sig_d_inv = sin(asd(3)) / cos(asd(3)); // double sig_d_inv = sin(asd(3)) / cos(asd(3)); Matrix3d R = gc_Rodriguez(aa); // av.head(3) = R.col(2) / sig_d_inv; av.head(3) = R.col(2) * sig_d_inv; av.tail(3) = R.col(0); return av; }
// 6. Total measurement update void Ekf::measurementUpdateDecaWave(Vector4d z){ // Determine h and H Vector4d h; h = hDecaWave(state_, DecaWaveBeaconLoc_, DecaWaveOffset_); Matrix46 H; H = HDecaWave(state_, DecaWaveBeaconLoc_, DecaWaveOffset_); // Find Kalman Gain // First check if any z values are -1 (i.e. a bad measurement). If so, make // the corresponding entry in RDecaWave_ very high for this iteration. This // effectively cancels out any influence by that measurement on the update. Matrix4d RDecaWaveTemp; for (int i = 0; i < z.rows(); i++) { if (z(i) < 0) { RDecaWaveTemp(i,i) = 1e20; } else { RDecaWaveTemp(i,i) = RDecaWave_(i,i); } } // Now go on to calculate the Kalman gain with the new R value. Matrix64 K; K = KDecaWave(cov_, H, RDecaWaveTemp); // Find new state state_ = stateUpdateDecaWave(state_, K, z, h); // Find new covariance cov_ = covUpdateDecaWave(cov_, K, H); cov_ = zeroOutBiasXYThetaCov(cov_); }
std::tuple<double, double> Triangle::uvCoords(const Vector4d &point) const { Vector4d edge1 = p2 - p1; Vector4d edge2 = p3 - p1; Vector4d h = cross(trueNormal, edge2); double a = edge1.dot(h); double f = 1/a; Vector4d s = point - p1; double u = f * s.dot(h); Vector4d q = cross(s, edge1); double v = f * q.dot(trueNormal); return std::make_tuple(u, v); }
void rigidBodyDynamics::setState(VectorXd x, Vector4d q) { _r = x.segment<3>(0); _v = x.segment<3>(3); _a = x.segment<3>(6); _w = x.segment<3>(9); _qref = q / q.norm(); }
bool Rotation3::FromQuaternion(const Vector4d& quat) { if (fabs(quat.norm() - 1.0) > 1e-6) { std::cout << "Rotation3::FromQuaternion: invalid quaternion" << std::endl; return false; } quat_ = quat; return true; }
ClothoidPtr ClothoidFitter::getCurveWithZeroCurvature(double param) const { Matrix<double, 5, 5> lhs; Matrix<double, 5, 1> rhs; Vector4d constraint; constraint << 6 * param, 2, 0, 0; // second derivative of ax^3+bx^2+cx+d is 6ax+2b //For constrained least squares, //lhs is now [A^T A C] // [ C^T 0] lhs << _getLhs(_totalLength), constraint, constraint.transpose(), 0; rhs << _rhs, 0; Vector4d abcd = (lhs.inverse() * rhs).head<4>(); return getClothoidWithParams(abcd); }