void absOrientRobustTukey(const int npts, const double pts1[], const double pts2[], double R[], double t[], int maxIter) {
	assert(npts >= 3);
	double* w = new double[npts];
	double* d = new double[npts];

	for (int i = 0; i < npts; i++)
		w[i] = 1.0;

	double ad = 0, sv = 0, th = 0;
	for (int k = 0; k < maxIter; k++) {
		absOrientWeighted(npts, w, pts1, pts2, R, t);
		if (k == 0) {
			//get average residual
			ad = 0;
			for (int i = 0; i < npts; i++) {
				double M[3];
				rigidTrans(R, t, pts1 + 3 * i, M);
				d[i] = dist3(M, pts2 + 3 * i);
				ad += d[i];
			}
			ad /= npts;

			//get standard deviation;
			sv = 0;
			for (int i = 0; i < npts; i++) {
				double v = d[i] - ad;
				sv += v * v;
			}

			sv /= (npts - 1);
			sv = sqrt(sv);

			th = ad + 3 * sv;
		}
		//logInfo("-------(%g,%g) --- \n", ad, sv);
		for (int i = 0; i < npts; i++) {
			if (k > 0) {
				double M[3];
				rigidTrans(R, t, pts1 + 3 * i, M);
				d[i] = dist3(M, pts2 + 3 * i);
			}

			if (d[i] > th) {
				w[i] = 0;
			} else {
				double dr = d[i] / th;
				w[i] = (1 - dr * dr);
				w[i] *= w[i];
			}
			//test
			//logInfo("[%d]:%g %g\n", i, d[i], w[i]);
		}
	}
	delete[] w;
	delete[] d;
}
double getCameraDistance(const double R1[9], const double t1[3],
		const double R2[9], const double t2[3]) {
	double org1[3], org2[3];
	getCameraCenter(R1, t1, org1);
	getCameraCenter(R2, t2, org2);
	return dist3(org1, org2);
}
示例#3
0
void FixedConstraint::Fix()
{
    rA = rAorig;
    rA.rot(bodyA->form->alpha);
    rB = rBorig;
    rB.rot(bodyB->form->alpha);

    Vector2 dist = bodyB->form->point + rB - bodyA->form->point - rA;

    double delta = dist.norm2() - init_dist_;

    if (abs(delta) > DBL_EPSILON)
    {
        dist.normalize2();

        Vector2 fix_dist = dist * delta;

        bodyA->form->point = bodyA->form->point + fix_dist * bodyA->iMass * 0.7;
        bodyB->form->point = bodyB->form->point - fix_dist * bodyB->iMass * 0.7;

        Vector3 dist3(fix_dist.v1, fix_dist.v2, 0);
        Vector3 rA3(rA.v1, rA.v2, 0);
        Vector3 rB3(rB.v1, rB.v2, 0);

        //bodyA->form->alpha += rA3.cross(dist3).v3 * bodyA->iInert * 0.01;
        //bodyB->form->alpha += rB3.cross(dist3).v3 * bodyB->iInert * 0.01;
    }
}
示例#4
0
void FixedConstraint::init()
{
    rA = rAorig;
    rA.rot(bodyA->form->alpha);
    rB = rBorig;
    rB.rot(bodyB->form->alpha);

    //std::cout << "Ra: " << rA.v1 << " " << rA.v2 << "   Rb: " << rB.v1 << " " << rB.v2 << std::endl;

    Vector2 dist = (bodyB->form->point + rB - bodyA->form->point - rA);
    dist.normalize2();

    Vector3 dist3(dist.v1, dist.v2, 0);
    Vector3 rA3(rA.v1, rA.v2, 0);
    Vector3 rB3(rB.v1, rB.v2, 0);

    A[0][0] = (bodyA->iMass + bodyB->iMass)
        + bodyA->iInert * (dist3.cross(rA3).v3) * (dist3.cross(rA3).v3)
        + bodyB->iInert * (dist3.cross(rB3).v3) * (dist3.cross(rB3).v3);

    rel_vel_ = dist * (bodyB->point_velocity(bodyB->form->point + rB)
        - bodyA->point_velocity(bodyA->form->point + rA));
    Eta[0] = -rel_vel_;

    /*if (rel_vel_ > 0.1)
        std::cout << "A: " << A[0][0] << "   rel vel: " << Eta[0] << std::endl;*/
}
示例#5
0
// TEST 6 - WORLD-TO-EXTRINSICS AND EXTRINSICS-TO-WORLD TRANSFORMATIONS
bool test6(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
{
  vcg::Matrix44d WtoE1 = shot1.GetWorldToExtrinsicsMatrix();
  vcg::Matrix44d WtoE2 = shot2.GetWorldToExtrinsicsMatrix();
  vcg::Matrix44d EtoW1 = shot1.GetExtrinsicsToWorldMatrix();
  vcg::Matrix44d EtoW2 = shot2.GetExtrinsicsToWorldMatrix();

  vcg::Matrix44d I1 = WtoE1 * EtoW1;
  vcg::Matrix44d I2 = WtoE2 * EtoW2;
  vcg::Matrix44d I3 = EtoW1 * WtoE1;
  vcg::Matrix44d I4 = EtoW2 * WtoE2;

  if (checkIdentity(I1) > precision)
    return false;

  if (checkIdentity(I2) > precision)
    return false;

  if (checkIdentity(I3) > precision)
    return false;

  if (checkIdentity(I4) > precision)
    return false;

  vcg::Point3d axisX(1.0, 0.0, 0.0);
  vcg::Point3d axisY(0.0, 1.0, 0.0);
  vcg::Point3d axisZ(0.0, 0.0, 1.0);

  vcg::Point3d vx = EtoW1 * axisX;
  vcg::Point3d vy = EtoW1 * axisY;
  vcg::Point3d vz = EtoW1 * axisZ;

  if (dist3(vx, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(0)) > precision)
    return false;

  if (dist3(vy, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(1)) > precision)
    return false;

  if (dist3(vz, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(2)) > precision)
    return false;

  return true;
}
示例#6
0
// TEST 3 - DEPTH COMPUTATION
///////////////////////////////////////////////////////////////////////////////
bool test3(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
{
  vcg::Point2d p1proj, p2proj;

  p1proj = shot1.Project(p1);
  p2proj = shot2.Project(p2);

  vcg::Point3d p1unproj, p2unproj;

  double depth1 = shot1.Depth(p1);
  double depth2 = shot2.Depth(p2);

  p1unproj = shot1.UnProject(p1proj, depth1);
  p2unproj = shot2.UnProject(p2proj, depth2);

  if (dist3(p1, p1unproj) > precision)
    return false;

  if (dist3(p2, p2unproj) > precision)
    return false;

  return true;
}
示例#7
0
// TEST 2 - PROJECTION AND UNPROJECTION
///////////////////////////////////////////////////////////////////////////////
bool test2(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
{
  vcg::Point2d p1proj, p2proj;

  p1proj = shot1.Project(p1);
  p2proj = shot2.Project(p2);

  vcg::Point3d p1unproj, p2unproj;
  vcg::Point3d pcam1, pcam2;

  pcam1 = shot1.ConvertWorldToCameraCoordinates(p1);
  p1unproj = shot1.UnProject(p1proj, pcam1[2]);
  pcam2 = shot2.ConvertWorldToCameraCoordinates(p2);
  p2unproj = shot2.UnProject(p2proj, pcam2[2]);

  if (dist3(p1, p1unproj) > precision)
    return false;

  if (dist3(p2, p2unproj) > precision)
    return false;

  return true;
}
void absOrientRobustL1(const int npts, const double pts1[], const double pts2[], double R[], double t[], int maxIter) {
	assert(npts >= 3);
	double* w = new double[npts];

	for (int i = 0; i < npts; i++)
		w[i] = 1.0;

	for (int k = 0; k < maxIter; k++) {
		absOrientWeighted(npts, w, pts1, pts2, R, t);
		//get the residual and recompute the weights
		logInfo("-------\n");
		for (int i = 0; i < npts; i++) {
			double M[3];
			rigidTrans(R, t, pts1 + 3 * i, M);
			double d = dist3(M, pts2 + 3 * i);
			w[i] = 1.0 / (d + 1e-6);
			//test
			//logInfo("[%d]:%g %g\n", i, d, w[i]);
		}
	}
	delete[] w;
}
示例#9
0
// Circular arc entity (Type 100)
bool
InputIges::read_100(IgesDirectoryEntry* de, bool check_only_status)
{
  bool result = true;

  static ecif_EdgeGeometry_X edge;
  init_trx_data(edge);

  edge.type = ECIF_CIRCLE;

  int i,j;
  int int_fld;
  double dbl_fldb[1];
  char chr_fld;

  bool is_closed;

  static Point3 points[3];

  locateParamEntry(de);

  //-Read one data-line
  readDataLine(lineBuffer, dataBuffer);
  istrstream* data_line = new istrstream(dataBuffer);

  //-Read center and two points
  *data_line >> int_fld >> chr_fld; // Entity-id
  readDoubleFields(data_line, 1, dbl_fldb); // z-inclination

  for(i = 0; i < 3; i++) {

    points[i][2] = 0.0;

    for (j = 0; j < 2; j++) {
      readDoubleFields(data_line, 1, dbl_fldb);
      points[i][j] = dbl_fldb[0];
    }
  }

  if ( isZero(dist3(points[1], points[2])) ) {
    is_closed = true;
  } else {
    is_closed = false;
  }

  if ( modelDimension == ECIF_2D && is_closed ) {
    de->canBeBody = true;
  }

  if ( check_only_status ) {
    return result;
  }

  // Create body element
  // ===================

  edge.location = new Point3[1];
  edge.start = new Point3[1];
  edge.end = new Point3[1];

  for (i = 0; i < 3; i++) {
    edge.location[0][i] = points[0][i];
    edge.start[0][i] = points[1][i];
    edge.end[0][i] = points[2][i];
  }

  int body_layer = 0;
  createBodyElement2D(de->body, body_layer, edge);

  reset_trx_data(edge);

  return result;
}
示例#10
0
struct pair_comp gen_pair_comp(double *alpha, double dist, gsl_rng *rg)
{
    struct pair_comp pc;
    double s1[3], min_v, max_dist = -1.0;
    unsigned i, min_i;
    double alpha_low[] = { 0.1, 0.1, 0.1, 0.1 };

    if (dist == 1)
    {
        unsigned long c1 = gsl_rng_uniform_int(rg, 4);
        unsigned long c2 = (c1 + 1 + gsl_rng_uniform_int(rg, 3)) % 4;
        memcpy(pc.c1, bary_corners[c1], sizeof(pc.c1));
        memcpy(pc.c2, bary_corners[c2], sizeof(pc.c2));
    }
    else if (dist == 0)
    {
        gsl_ran_dirichlet(rg, 4, (dist > 0.9 ? alpha_low : alpha), pc.c1);
        memcpy(pc.c2, pc.c1, sizeof(pc.c2));
    }
    else if (dist > 0.95)
    {
        fprintf(stderr, "Cannot handle distances not equal to 1 but > 0.95\n");
        exit(1);
    }
    else
    {
        /* generate a valid c1 */
        double s_corner[3];
        while (max_dist < dist * (1.0 / 0.95))
        {
            gsl_ran_dirichlet(rg, 4, (dist > 0.9 ? alpha_low : alpha), pc.c1);
            min_v = pc.c1[0], min_i = 0;
            for (i = 1; i != 4; ++i)
                if (min_v > pc.c1[i])
                    min_v = pc.c1[i], min_i = i;
            barycentric_to_simplex(pc.c1, s1);
            barycentric_to_simplex(bary_corners[min_i], s_corner);
            max_dist = dist3(s_corner, s1);
        }
    
        /* sample from the unit sphere until we find a point in the
           simplex */
        double sph[3], s1p[3];

        while (1)
        {
            unit_sphere3(sph, rg);
            for (i = 0; i != 3; ++i) s1p[i] = s1[i] + dist * sph[i];
            if (inside_simplex(s1p)) break;
        }
        simplex_to_barycentric(s1p, pc.c2);
    }
    /* fprintf(stderr,  */
    /*         "%5.3f,%5.3f,%5.3f,%5.3f\t" */
    /*         "%5.3f,%5.3f,%5.3f,%5.3f\t" */
    /*         "%f\t%f\n",  */
    /*         pc.c1[0], pc.c1[1], pc.c1[2], pc.c1[3], */
    /*         pc.c2[0], pc.c2[1], pc.c2[2], pc.c2[3], */
    /*         dist,  */
    /*         dist4_scaled(pc.c1, pc.c2) */
    /*         ); */
    return pc;
}
示例#11
0
int SingleSLAM::newMapPoints(std::vector<MapPoint*>& mapPts, double maxEpiErr,
		double maxNcc) {
	std::vector<FeaturePoint*> vecFeatPts;
	getUnMappedAndTrackedFeatPts(vecFeatPts, 0, Param::nMinFeatTrkLen);

	mapPts.clear();
	mapPts.reserve(4096);

	double M[3], m1[2], m2[2];

	//reconstruct 3D map points
	int numRecons = 0;
	for (size_t k = 0; k < vecFeatPts.size(); k++) {
		FeaturePoint* cur_fp = vecFeatPts[k];
		FeaturePoint* pre_fp = cur_fp;
		while (pre_fp->preFrame && pre_fp->preFrame->cam) {
			if (pre_fp->type == TYPE_FEATPOINT_DYNAMIC) {
				break;
			}
			pre_fp = pre_fp->preFrame;
		}
		if (pre_fp->type == TYPE_FEATPOINT_DYNAMIC || !pre_fp->cam)
			continue;

		normPoint(iK.data, pre_fp->m, m1);
		normPoint(iK.data, cur_fp->m, m2);

		//triangulate the two feature points to get the 3D point
		binTriangulate(pre_fp->cam->R, pre_fp->cam->t, cur_fp->cam->R,
				cur_fp->cam->t, m1, m2, M);

		if (isAtCameraBack(cur_fp->cam->R, cur_fp->cam->t, M))
			continue;

		double cov[9], org[3];
		getBinTriangulateCovMat(K.data, pre_fp->cam->R, pre_fp->cam->t, K.data,
				cur_fp->cam->R, cur_fp->cam->t, M, cov, Const::PIXEL_ERR_VAR);
		getCameraCenter(cur_fp->cam->R, cur_fp->cam->t, org);
		double s = fabs(cov[0] + cov[4] + cov[8]);
		if (dist3(org, M) < sqrt(s))
			continue;

		//check the reprojection error
		double err1 = reprojErrorSingle(K.data, pre_fp->cam->R, pre_fp->cam->t,
				M, pre_fp->m);
		double err2 = reprojErrorSingle(K.data, cur_fp->cam->R, cur_fp->cam->t,
				M, cur_fp->m);

		if (err1 < maxEpiErr && err2 < maxEpiErr) {
			//a new map point is generated
			refineTriangulation(cur_fp, M, cov);
			err1 = reprojErrorSingle(K.data, pre_fp->cam->R, pre_fp->cam->t, M,
					pre_fp->m);
			err2 = reprojErrorSingle(K.data, cur_fp->cam->R, cur_fp->cam->t, M,
					cur_fp->m);
			if (isAtCameraBack(cur_fp->cam->R, cur_fp->cam->t, M)
					|| isAtCameraBack(pre_fp->cam->R, pre_fp->cam->t, M))
				continue;
			if (err1 < maxEpiErr && err2 < maxEpiErr) {
				MapPoint* pM = new MapPoint(M[0], M[1], M[2], pre_fp->f);
				doubleArrCopy(pM->cov, 0, cov, 9);
				mapPts.push_back(pM);
				pM->lastFrame = cur_fp->f;
				for (FeaturePoint* p = cur_fp; p; p = p->preFrame)
					p->mpt = pM;

				//add the feature point
				pM->addFeature(camId, cur_fp);
				pM->setLocalStatic();

				//compute the NCC block
				pM->nccBlks[camId].computeScaled(m_lastKeyPos->imgSmall,
						m_lastKeyPos->imgScale, cur_fp->x, cur_fp->y);

				int x = max(0, min((int) cur_fp->x, m_rgb.w - 1));
				int y = max(0, min((int) cur_fp->y, m_rgb.h - 1));
				pM->setColor(m_rgb(x, y));
				numRecons++;
			}
		}
	}
	return numRecons;
}
double ptoline(point3 p,point3 l1,point3 l2)
{
	return vlen(xmult(subt(p,l1),subt(l2,l1)))/dist3(l1,l2);
}
double ptoline(point3 p,line3 l)
{
	return vlen(xmult(subt(p,l.a),subt(l.b,l.a)))/dist3(l.a,l.b);
}
示例#14
0
static int 
maxmatch(v_data * graph,	/* array of vtx data for graph */
	ex_vtx_data * geom_graph,	/* array of vtx data for graph */
	int nvtxs,	/* number of vertices in graph */
	int *mflag,	/* flag indicating vtx selected or not */
	int dist2_limit
    )
/* 
    Compute a matching of the nodes set. 
    The matching is not based only on the edge list of 'graph', 
    which might be too small, 
    but on the wider edge list of 'geom_graph' (which includes 'graph''s edges)

    We match nodes that are close both in the graph-theoretical sense and 
    in the geometry sense  (in the layout)
*/
{
    int *order;			/* random ordering of vertices */
    int *iptr, *jptr;		/* loops through integer arrays */
    int vtx;			/* vertex to process next */
    int neighbor;		/* neighbor of a vertex */
    int nmerged = 0;		/* number of edges in matching */
    int i, j;			/* loop counters */
    float max_norm_edge_weight;
    double inv_size;
    double *matchability = N_NEW(nvtxs, double);
    double min_edge_len;
    double closest_val = -1, val;
    int closest_neighbor;
    float *vtx_vec = N_NEW(nvtxs, float);
    float *weighted_vtx_vec = N_NEW(nvtxs, float);
    float sum_weights;

    // gather statistics, to enable normalizing the values
    double avg_edge_len = 0, avg_deg_2 = 0;
    int nedges = 0;

    for (i = 0; i < nvtxs; i++) {
	avg_deg_2 += graph[i].nedges;
	for (j = 1; j < graph[i].nedges; j++) {
	    avg_edge_len += ddist(geom_graph, i, graph[i].edges[j]);
	    nedges++;
	}
    }
    avg_edge_len /= nedges;
    avg_deg_2 /= nvtxs;
    avg_deg_2 *= avg_deg_2;

    // the normalized edge weight of edge <v,u> is defined as:
    // weight(<v,u>)/sqrt(size(v)*size(u))
    // Now we compute the maximal normalized weight
    if (graph[0].ewgts != NULL) {
	max_norm_edge_weight = -1;
	for (i = 0; i < nvtxs; i++) {
	    inv_size = sqrt(1.0 / geom_graph[i].size);
	    for (j = 1; j < graph[i].nedges; j++) {
		if (graph[i].ewgts[j] * inv_size /
		    sqrt((float) geom_graph[graph[i].edges[j]].size) >
		    max_norm_edge_weight) {
		    max_norm_edge_weight =
			(float) (graph[i].ewgts[j] * inv_size /
				 sqrt((double)
				      geom_graph[graph[i].edges[j]].size));
		}
	    }
	}
    } else {
	max_norm_edge_weight = 1;
    }

    /* Now determine the order of the vertices. */
    iptr = order = N_NEW(nvtxs, int);
    jptr = mflag;
    for (i = 0; i < nvtxs; i++) {
	*(iptr++) = i;
	*(jptr++) = -1;
    }

    // Option 1: random permutation
#if 0
    int temp;
    for (i=0; i<nvtxs-1; i++) {
          // use long_rand() (not rand()), as n may be greater than RAND_MAX
	j=i+long_rand()%(nvtxs-i); 
	temp=order[i];
	order[i]=order[j];
	order[j]=temp;
    }
#endif
    // Option 2: sort the nodes begining with the ones highly approriate for matching

#ifdef DEBUG
    srand(0);
#endif
    for (i = 0; i < nvtxs; i++) {
	vtx = order[i];
	matchability[vtx] = graph[vtx].nedges;	// we less want to match high degree nodes
	matchability[vtx] += geom_graph[vtx].size;	// we less want to match large sized nodes
	min_edge_len = 1e99;
	for (j = 1; j < graph[vtx].nedges; j++) {
	    min_edge_len =
		MIN(min_edge_len,
		    ddist(geom_graph, vtx,
			 graph[vtx].edges[j]) / avg_edge_len);
	}
	matchability[vtx] += min_edge_len;	// we less want to match distant nodes
	matchability[vtx] += ((double) rand()) / RAND_MAX;	// add some randomness 
    }
    quicksort_place(matchability, order, 0, nvtxs - 1);
    free(matchability);

    // Start determining the matched pairs
    for (i = 0; i < nvtxs; i++) {
	vtx_vec[i] = 0;
    }
    for (i = 0; i < nvtxs; i++) {
	weighted_vtx_vec[i] = 0;
    }

    // relative weights of the different criteria
    for (i = 0; i < nvtxs; i++) {
	vtx = order[i];
	if (mflag[vtx] >= 0) {	/*  already matched. */
	    continue;
	}
	inv_size = sqrt(1.0 / geom_graph[vtx].size);
	sum_weights = fill_neighbors_vec(graph, vtx, weighted_vtx_vec);
	fill_neighbors_vec_unweighted(graph, vtx, vtx_vec);
	closest_neighbor = -1;

	/*
	   We match node i with the "closest" neighbor, based on 4 criteria:
	   (1) (Weighted) fraction of common neighbors  (measured on orig. graph)
	   (2) AvgDeg*AvgDeg/(deg(vtx)*deg(neighbor)) (degrees measured on orig. graph)
	   (3) AvgEdgeLen/dist(vtx,neighbor)
	   (4) Weight of normalized direct connection between nodes (measured on orig. graph)
	 */

	for (j = 1; j < geom_graph[vtx].nedges; j++) {
	    neighbor = geom_graph[vtx].edges[j];
	    if (mflag[neighbor] >= 0) {	/*  already matched. */
		continue;
	    }
	    // (1): 
	    val =
		A * unweighted_common_fraction(graph, vtx, neighbor,
					       vtx_vec);

	    if (val == 0 && (dist2_limit || !dist3(graph, vtx, neighbor))) {
		// graph theoretical distance is larger than 3 (or 2 if '!dist3(graph, vtx, neighbor)' is commented)
		// nodes cannot be matched
		continue;
	    }
	    // (2)
	    val +=
		B * avg_deg_2 / (graph[vtx].nedges *
				 graph[neighbor].nedges);


	    // (3)
	    val += C * avg_edge_len / ddist(geom_graph, vtx, neighbor);

	    // (4)
	    val +=
		(weighted_vtx_vec[neighbor] * inv_size /
		 sqrt((float) geom_graph[neighbor].size)) /
		max_norm_edge_weight;



	    if (val > closest_val || closest_neighbor == -1) {
		closest_neighbor = neighbor;
		closest_val = val;
	    }

	}
	if (closest_neighbor != -1) {
	    mflag[vtx] = closest_neighbor;
	    mflag[closest_neighbor] = vtx;
	    nmerged++;
	}
	empty_neighbors_vec(graph, vtx, vtx_vec);
	empty_neighbors_vec(graph, vtx, weighted_vtx_vec);
    }

    free(order);
    free(vtx_vec);
    free(weighted_vtx_vec);
    return (nmerged);
}