C++ Vector2d::dot方法代码示例

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

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

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

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

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

/**
Eigen::Vector2d internal_ray_lens_intersection( const Eigen::Vector2d &direction_of_internal, const Eigen::Vector2d &origin_of_internal, const Eigen::Vector2d &centerpoint_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 &centerpoint_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;
}

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

/*------------------------------------------------------------------------------*/
FM_INLINE
FastMarchingVertex * UnfoldTriangle(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
	const Eigen::MatrixXi &TT, const  Eigen::MatrixXi &TTi,
	std::vector<struct FastMarchingVertex> &vertices,
	int f, int v, int v1, int v2,
	FM_Float& dist, FM_Float& dot1, FM_Float& dot2)
{
	const Eigen::Vector3d& p = V.row(v);// vert.GetPosition();
	const Eigen::Vector3d& p1 = V.row(v1);//vert1.GetPosition();
	const Eigen::Vector3d& p2 = V.row(v2);//vert2.GetPosition();

	Eigen::Vector3d e1 = p1 - p;
	FM_Float rNorm1 = e1.norm(); //~e1
	e1.normalize(); // e1 /= rNorm1;
	Eigen::Vector3d e2 = p2 - p;
	FM_Float rNorm2 = e2.norm(); // ~e2;
	e2.normalize(); // e2 /= rNorm2;

	FM_Float dot = e1.adjoint()*e2;// e1*e2;
	FM_ASSERT(dot < 0);

	/* the equation of the lines defining the unfolding region [e.g. line 1 : {x ; <x,eq1>=0} ]*/
	Eigen::Vector2d eq1 = Eigen::Vector2d(dot, sqrt(1 - dot*dot));
	Eigen::Vector2d eq2 = Eigen::Vector2d(1, 0);

	/* position of the 2 points on the unfolding plane */
	Eigen::Vector2d x1(rNorm1, 0);
	Eigen::Vector2d x2 = eq1*rNorm2;

	/* keep track of the starting point */
	Eigen::Vector2d xstart1 = x1;
	Eigen::Vector2d xstart2 = x2;

	FastMarchingVertex* pV1 = &(vertices[v1]);
	FastMarchingVertex* pV2 = &(vertices[v2]);

	
	int cur_f, cur_v;
	
	get_oppisite_f_v(F, TT,TTi, f,v, cur_f, cur_v);

	//FM_GeodesicFace* pCurFace = (FM_GeodesicFace*)CurFace.GetFaceNeighbor(vert);


	int nNum = 0;
	while (nNum < 50 && cur_f != -1) // NULL)
	{
		//	FastMarchingVertex* pV = (FastMarchingVertex*)pCurFace->GetVertex(*pV1, *pV2); //opposite vertex to face and edge(pV1,pV2)
		//	FM_ASSERT(pV != NULL);
		FastMarchingVertex* pV = &(vertices[cur_v]); //opposite vertex to face and vert
													 /*
													 e1 = pV2->GetPosition() - pV1->GetPosition();
													 FM_Float rNorm1 = ~e1;
													 e1 /= rNorm1;
													 e2 = pV->GetPosition() - pV1->GetPosition();
													 FM_Float rNorm2 = ~e2;
													 e2 /= rNorm2;
													 */

		Eigen::Vector3d e1 = V.row(pV2->vid) - V.row(pV1->vid);
		FM_Float rNorm1 = e1.norm(); //~e1
		e1.normalize(); // e1 /= rNorm1;
		Eigen::Vector3d e2 = V.row(pV->vid) - V.row(pV1->vid);
		FM_Float rNorm2 = e2.norm(); // ~e2;
		e2.normalize(); // e2 /= rNorm2;

						/* compute the position of the new point x on the unfolding plane (via a rotation of -alpha on (x2-x1)/rNorm1 )
						| cos(alpha) sin(alpha)|
						x = |-sin(alpha) cos(alpha)| * [x2-x1]*rNorm2/rNorm1 + x1   where cos(alpha)=dot
						*/
		Eigen::Vector2d vv = (x2 - x1)*rNorm2 / rNorm1;
		dot = e1.adjoint()*e2;  //e1*e2;
								//	Eigen::Vector2d x = vv.Rotate2D();////vv.Rotate(-acos(dot)) + x1;

		Eigen::Rotation2D<double> rot2(-acos(dot));
		Eigen::Vector2d x = rot2*vv + x1;  //dhw to check


										   /* compute the intersection points.
										   We look for x=x1+lambda*(x-x1) or x=x2+lambda*(x-x2) with <x,eqi>=0, so */
		FM_Float lambda11 = -(x1.dot(eq1)) / ((x - x1).dot(eq1));	 //-(x1*eq1) / ((x - x1)*eq1);	// left most 
		FM_Float lambda12 = -(x1.dot(eq2)) / ((x - x1).dot(eq2));  //-(x1*eq2) / ((x - x1)*eq2);	// right most
		FM_Float lambda21 = -(x2.dot(eq1)) / ((x - x2).dot(eq1)); //-(x2*eq1) / ((x - x2)*eq1);	// left most 
		FM_Float lambda22 = -(x2.dot(eq2)) / ((x - x2).dot(eq2));   //-(x2*eq2) / ((x - x2)*eq2);	// right most
		bool bIntersect11 = (lambda11 >= 0) && (lambda11 <= 1);
		bool bIntersect12 = (lambda12 >= 0) && (lambda12 <= 1);
		bool bIntersect21 = (lambda21 >= 0) && (lambda21 <= 1);
		bool bIntersect22 = (lambda22 >= 0) && (lambda22 <= 1);
		if (bIntersect11 && bIntersect12)
		{
			//			FM_ASSERT( !bIntersect21 && !bIntersect22 );
			/* we should unfold on edge [x x1] */
			//	pCurFace = (FM_GeodesicFace*)pCurFace->GetFaceNeighbor(*pV2);
			f = cur_f;
			get_oppisite_f_v(F, TT,TTi, f, pV2->vid, cur_f, cur_v);

			pV2 = pV;
			x2 = x;
		}
//.........这里部分代码省略.........

void KeyEdge::prepareSculptPreserveTangents_()
{
    const bool preserveTangentEdges = false;
    if (!preserveTangentEdges)
        return;


    // find instant edges where tangency must be preserved
    sculpt_beginLeftDer_ = geometry()->der(0);
    sculpt_beginRightDer_ = geometry()->der(geometry()->length());
    sculpt_keepRightAsLeft_.clear();
    sculpt_keepLeftAsLeft_.clear();
    sculpt_keepLeftAsRight_.clear();
    sculpt_keepRightAsRight_.clear();
    sculpt_keepMyselfTangent_ = false;
    if (preserveTangentEdges)
    {
        double dotThreshold = 0.9;
        if(startVertex_)
        {
            KeyEdgeSet leftEdges =  startVertex_->spatialStar();
            leftEdges.remove(this);
            foreach(KeyEdge * ie, leftEdges)
            {
                if(ie->endVertex_ == startVertex_)
                {
                    Eigen::Vector2d rightDer = ie->geometry()->der(ie->geometry()->length());
                    double dot = rightDer.dot(sculpt_beginLeftDer_);
                    if(dot > dotThreshold)
                        sculpt_keepRightAsLeft_ << ie;
                }
                if(ie->startVertex_ == startVertex_)
                {
                    Eigen::Vector2d rightDer = - ie->geometry()->der(0);
                    double dot = rightDer.dot(sculpt_beginLeftDer_);
                    if(dot > dotThreshold)
                        sculpt_keepLeftAsLeft_ << ie;
                }
            }
        }
        if(endVertex_)
        {
            KeyEdgeSet rightEdges =  endVertex_->spatialStar();
            rightEdges.remove(this);
            foreach(KeyEdge * ie, rightEdges)
            {
                // For now, disabling tangent preservation for 0-edge loops
                if(ie == this)
                    continue;

                if(ie->startVertex_ == endVertex_)
                {
                    Eigen::Vector2d leftDer = ie->geometry()->der(0);
                    double dot = leftDer.dot(sculpt_beginRightDer_);
                    if(dot > dotThreshold)
                        sculpt_keepLeftAsRight_ << ie;
                }
                if(ie->endVertex_ == endVertex_)
                {
                    Eigen::Vector2d leftDer = - ie->geometry()->der(ie->geometry()->length());
                    double dot = leftDer.dot(sculpt_beginRightDer_);
                    if(dot > dotThreshold)
                        sculpt_keepRightAsRight_ << ie;
                }
            }
        }
        if(endVertex_ && (endVertex_ == startVertex_))
        {
            // For now, disabling tangent preservation for 0-edge loops
            // What's below doesn't work, because continueSculpt assume
            // no resampling has occured since prepareSculpt

            //double dot = sculpt_beginRightDer_.dot(sculpt_beginLeftDer_);
            //if(dot > dotThreshold)
            //{
            //    sculpt_keepMyselfTangent_ = true;
            //}
        }
    }
}

Sophus::SE3d Registrator3P::solve(const ptam::KeyFrame& kfa, const ptam::KeyFrame& kfb,
                                  const std::vector<cv::DMatch>& matches)
{
    if (matches.size() < 5)
        return Sophus::SE3d();

    // Generate nHyp_ hypotheses:
    std::vector<Hypothesis3P> hyp;
    nHyp_ = matches.size() * N_SAMPLES;
    while (hyp.size() < nHyp_) {
        std::vector<int> sampleInd;

        // choose x random matches
        randSampleNoReplacement(matches.size(), N_SAMPLES, sampleInd);

        std::vector<cv::Point3f> mapPointsA(N_SAMPLES);
        std::vector<cv::Point2f> imagePointsB(N_SAMPLES);

        for (int i = 0; i < N_SAMPLES; i++) {
            int ind = sampleInd[i];
            const Eigen::Vector3d& mpa = cs_geom::toEigenVec(kfa.mapPoints[matches[ind].queryIdx]->v3RelativePos);
            mapPointsA[i]   = cv::Point3f(mpa[0], mpa[1], mpa[2]);
            int scale = 1;// << kfb.keypoints[matches[ind].trainIdx].octave;
            imagePointsB[i].x = kfb.keypoints[matches[ind].trainIdx].pt.x;// * scale;
            imagePointsB[i].y = kfb.keypoints[matches[ind].trainIdx].pt.y;// * scale;

        }

        cv::Mat rvec(3,1,cv::DataType<double>::type);
        cv::Mat tvec(3,1,cv::DataType<double>::type);
        /// here we need the model of camera B!
        int camNum = kfb.nSourceCamera;
//        cout << "cv::solvePnP... cam " << camNum << endl;
        if (cv::solvePnP(mapPointsA, imagePointsB, cam_[camNum].K(), cam_[camNum].D(), rvec, tvec,  false, cv::EPNP)) {
            Sophus::SE3d se3;
            Eigen::Vector3d r;
            cv::cv2eigen(rvec, r);
            cv::cv2eigen(tvec, se3.translation());
            se3.so3() = Sophus::SO3d::exp(r);
            hyp.push_back(Hypothesis3P(se3));
        }
//        cout << "cv::solvePnP finish..." << endl;
    }
    if (!hyp.size())
        return Sophus::SE3d();

    // Preemptive scoring
    // determine the order in which observations will be evaluated
    std::vector<int> oInd;
    randPerm(matches.size(),oInd);

    // start preemptive scoring
    int i = 0;
    int pr = preemption(i, nHyp_, blockSize_);
    while (i < nMaxObs_ && i < (int) matches.size() && pr > 1) {
        // observation oInd(i) consists of one pair of points:

        const cv::DMatch& match = matches[oInd[i]];
        const Eigen::Vector3d& mpa   = cs_geom::toEigenVec(kfa.mapPoints[match.queryIdx]->v3RelativePos);
        const cv::Point2f&     imbcv = kfb.keypoints[match.trainIdx].pt;
        int scale = 1;// << kfb.keypoints[match.trainIdx].octave;
        Eigen::Vector2d imb(imbcv.x * scale, imbcv.y * scale);

        // update score for all hypotheses w.r.t. observation oInd(i)
        for (int h = 0; h < (int) hyp.size(); h++) {
            Eigen::Vector2d err = cam_[kfb.nSourceCamera].project3DtoPixel(hyp[h].relPose*mpa) - imb;
            hyp[h].score -= log(1.0 + err.dot(err));
//            hyp[h].score += err.dot(err) < 3.0;
        }

        i++;
        int prnext = preemption(i, nHyp_, blockSize_);
        if (prnext != pr) {
            // select best hypotheses
            std::nth_element(hyp.begin(), hyp.begin() + prnext, hyp.end(), Hypothesis3P::compare);
            // now the first prnext elements of h contain the best hypotheses, erase the rest
            hyp.erase(hyp.begin() + prnext, hyp.end());
        }
        pr = prnext;
    }
    // preemptive scoring is done

    // select the single best hypothesis of possibly more than one remaining
    std::nth_element(hyp.begin(),hyp.begin() + 1, hyp.end(), Hypothesis3P::compare);

//    std::cout << "preemptive scoring using " << i << " observations done." <<  std::endl;
//    std::cout << hyp[0].relPose.inverse().matrix();
    return hyp[0].relPose.inverse();
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double sigma2, unsigned &row)
{
  unsigned nInt = m_data->interior.size ();
  bool wFunction (true);
  double ds = 1.0 / (2.0 * sigma2);
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  unsigned i1 (0);
  unsigned i2 (0);

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < m_data->interior_param.size ())
    {
      param = inverseMapping (m_nurbs, pcp, m_data->interior_param[p], error, pt, t, in_max_steps, in_accuracy);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, in_max_steps, in_accuracy);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR)
    {
      if (m_data->interior_rho.size () != nInt)
      {
        printf ("[FittingCurve2dSDM::assembleInterior] ERROR: size does not match\n");
      }
      else
      {
        n_prev = m_data->interior_normals[p];
        t_prev = m_data->interior_tangents[p];
        rho_prev = m_data->interior_rho[p];
        //        m_data->interior_normals[p] = n;
        //        m_data->interior_tangents[p] = t;
        //        m_data->interior_rho[p] = rho;
      }
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    double d;
    if ((pcp - pt).dot (n) >= 0.0)
    {
      d = (pcp - pt).norm ();
//.........这里部分代码省略.........

double
FittingCurve2d::inverseMapping (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, const double &hint,
                                double &error, Eigen::Vector2d &p, Eigen::Vector2d &t, double rScale, int maxSteps,
                                double accuracy, bool quiet)
{
  if (nurbs.Order () == 2)
    return inverseMappingO2 (nurbs, pt, error, p, t);

  int cp_red = (nurbs.m_order - 2);
  int ncpj = (nurbs.m_cv_count - 2 * cp_red);
  double pointAndTangents[4];

  double current, delta;
  Eigen::Vector2d r;
  std::vector<double> elements = getElementVector (nurbs);
  double minU = elements[0];
  double maxU = elements[elements.size () - 1];

  current = hint;

  for (int k = 0; k < maxSteps; k++)
  {

    nurbs.Evaluate (current, 1, 2, pointAndTangents);

    p (0) = pointAndTangents[0];
    p (1) = pointAndTangents[1];

    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];

    r = p - pt;

    // step width control
    int E = findElement (current, elements);
    double e = elements[E + 1] - elements[E];

    delta = -(0.5 * e * rScale) * r.dot (t) / t.norm (); //  A.ldlt().solve(b);

    //    e = 0.5 * std::abs<double> (e);
    //    if (delta > e)
    //      delta = e;
    //    if (delta < -e)
    //      delta = -e;

    if (std::abs<double> (delta) < accuracy)
    {

      error = r.norm ();
      return current;

    }
    else
    {
      current = current + delta;

      if (current < minU)
        current = maxU - (minU - current);
      else if (current > maxU)
        current = minU + (current - maxU);

    }

  }

  error = r.norm ();

  if (!quiet)
  {
    printf ("[FittingCurve2d::inverseMapping] Warning: Method did not converge (%e %d).\n", accuracy, maxSteps);
    printf ("[FittingCurve2d::inverseMapping] hint: %f current: %f delta: %f error: %f\n", hint, current, delta, error);
  }

  return current;
}

Eigen::Vector2d final_ray_scene_intersection( const Eigen::Vector2d &scene_point, const Eigen::Vector2d &final_ray_normalized, const Eigen::Vector2d &intersection_point_on_curved_surface)
{
	double d = (scene_point-intersection_point_on_curved_surface).dot(Eigen::Vector2d(0,-1))/(final_ray_normalized.dot(Eigen::Vector2d(0,-1)));
	return final_ray_normalized*d+intersection_point_on_curved_surface;
}