Beispiel #1
0
void VisualOdometry::confirmTrials() {
	// empty check
	if (trials.empty())
		return;
	// triangulate landmarks
	// prepare points
	cv::Mat pts1(2, trials.size(), cv::DataType<float>::type),
			pts2(2, trials.size(), cv::DataType<float>::type);
	int col = 0;
	for (auto it = trials.begin(); it != trials.end(); it++) {
		cv::KeyPoint k1, k2;
		it->firstPointPair(k1, k2);
		pts1.at<float>(0, col) = k1.pt.x;
		pts1.at<float>(1, col) = k1.pt.y;
		pts2.at<float>(0, col) = k2.pt.x;
		pts2.at<float>(1, col) = k2.pt.y;
		col++;
	}
	cv::Mat pts3D;
	triangulate(pts1, pts2, pts3D);

	// convert to world coordinate
	int sframe = trials.back().getStartFrame();
	cv::Mat posR, posT;
	graph.getPose(sframe, posR, posT);
	cv::Mat posRT = fullRT(posR, posT);
	pts3D = posRT * pts3D;

	int colcount = 0;
	for (auto it = trials.begin(); it != trials.end(); it++) {
		// add location to landmark data structure
		cv::Point3f p = cv::Point3f(pts3D.at<float>(0, colcount),
				pts3D.at<float>(1, colcount),
				pts3D.at<float>(2, colcount));
		it->setLocation(p);
		colcount++;

		// add initial value of land mark to factor graph data structure
		graph.addLandMark(landmarkID, p);

		// move from trail to true landmarks
		landmarks[landmarkID] = *it;

		// add factors
		int curframe = it->getStartFrame();
		for (int i = 0; i < it->getTraceSize(); i++) {
			cv::KeyPoint p1, p2;
			it->getPointPair(i, p1, p2);
			graph.addStereo(curframe, landmarkID, p1.pt, p2.pt);
			curframe++;
		}

		landmarkID++;
	}
	trials.clear();
}
Mat GetFundamentalMat(const vector<TrackedPoint> & trackedPoints,
		      vector<TrackedPoint>* inliers,
		      double ransac_max_dist, double ransac_p) {
  //vector<TrackedPoint> trackedPoints;
  //vector<matf> H_out;
  //NormalizePoints2(trackedPoints_, trackedPoints, H_out);
  const int method = FM_RANSAC;
  //const int method = FM_LMEDS;
  const double param1 = ransac_max_dist;
  const double param2 = ransac_p;
#ifdef OPENCV_2_1
  matf pts1(trackedPoints.size(), 2), pts2(trackedPoints.size(), 2);
  for (size_t i = 0; i < trackedPoints.size(); ++i) {
    pts1(i, 0) = trackedPoints[i].x1;
    pts2(i, 1) = trackedPoints[i].y1;
    pts2(i, 0) = trackedPoints[i].x2;
    pts2(i, 1) = trackedPoints[i].y2;
  }
  CvMat pts1cv = pts1;
  CvMat pts2cv = pts2;
  matf mat(3, 3);
  CvMat matcv = mat;
  Mat_<uchar> status(1, trackedPoints.size());
  CvMat statuscv = status;
  cvFindFundamentalMat(&pts1cv, &pts2cv, &matcv, method, param1, param2, &statuscv);
#else
  Mat_<Vec2f> pts1(trackedPoints.size(), 1), pts2(trackedPoints.size(), 1);
  for (size_t i = 0; i < trackedPoints.size(); ++i) {
    pts1(i, 0) = Vec2f(trackedPoints[i].x1, trackedPoints[i].y1);
    pts2(i, 0) = Vec2f(trackedPoints[i].x2, trackedPoints[i].y2);
  }
  vector<unsigned char> status;
  Mat mat = findFundamentalMat(pts1, pts2, method, param1, param2, status);
#endif
  if (inliers) {
    inliers->clear();
    for (size_t i = 0; i < trackedPoints.size(); ++i)
#ifdef OPENCV_2_1
      if (status(0, i))
#else
      if (status[i])
#endif
	inliers->push_back(trackedPoints[i]);
  }
  //return H_out[1].inv().t() * (matf)mat * H_out[0].inv();
  return mat;
}
Beispiel #3
0
int spheroidsToSTL(const string& out, const shared_ptr<DemField>& dem, Real tol, const string& solid, int mask, bool append, bool clipCell, bool merge){
	if(tol==0 || isnan(tol)) throw std::runtime_error("tol must be non-zero.");
	#ifndef WOO_GTS
		if(merge) throw std::runtime_error("woo.triangulated.spheroidsToSTL: merge=True only possible in builds with the 'gts' feature.");
	#endif
	// first traversal to find reference radius
	auto particleOk=[&](const shared_ptr<Particle>&p){ return (mask==0 || (p->mask & mask)) && (p->shape->isA<Sphere>() || p->shape->isA<Ellipsoid>() || p->shape->isA<Capsule>()); };
	int numTri=0;

	if(tol<0){
		LOG_DEBUG("tolerance is negative, taken as relative to minimum radius.");
		Real minRad=Inf;
		for(const auto& p: *dem->particles){
			if(particleOk(p)) minRad=min(minRad,p->shape->equivRadius());
		}
		if(isinf(minRad) || isnan(minRad)) throw std::runtime_error("Minimum radius not found (relative tolerance specified); no matching particles?");
		tol=-minRad*tol;
		LOG_DEBUG("Minimum radius "<<minRad<<".");
	}
	LOG_DEBUG("Triangulation tolerance is "<<tol);
	
	std::ofstream stl(out,append?(std::ofstream::app|std::ofstream::binary):std::ofstream::binary); // binary better, anyway
	if(!stl.good()) throw std::runtime_error("Failed to open output file "+out+" for writing.");

	Scene* scene=dem->scene;
	if(!scene) throw std::logic_error("DEM field has not associated scene?");

	// periodicity, cache that for later use
	AlignedBox3r cell;

	/*
	wasteful memory-wise, but we need to store the whole triangulation in case *merge* is in effect,
	when it is only an intermediary result and will not be output as-is
	*/
	vector<vector<Vector3r>> ppts;
	vector<vector<Vector3i>> ttri;
	vector<Particle::id_t> iid;

	for(const auto& p: *dem->particles){
		if(!particleOk(p)) continue;
		const auto sphere=dynamic_cast<Sphere*>(p->shape.get());
		const auto ellipsoid=dynamic_cast<Ellipsoid*>(p->shape.get());
		const auto capsule=dynamic_cast<Capsule*>(p->shape.get());
		vector<Vector3r> pts;
		vector<Vector3i> tri;
		if(sphere || ellipsoid){
			Real r=sphere?sphere->radius:ellipsoid->semiAxes.minCoeff();
			// 1 is for icosahedron
			int tess=ceil(M_PI/(5*acos(1-tol/r)));
			LOG_DEBUG("Tesselation level for #"<<p->id<<": "<<tess);
			tess=max(tess,0);
			auto uSphTri(CompUtils::unitSphereTri20(/*0 for icosahedron*/max(tess-1,0)));
			const auto& uPts=std::get<0>(uSphTri); // unit sphere point coords
			pts.resize(uPts.size());
			const auto& node=(p->shape->nodes[0]);
			Vector3r scale=(sphere?sphere->radius*Vector3r::Ones():ellipsoid->semiAxes);
			for(size_t i=0; i<uPts.size(); i++){
				pts[i]=node->loc2glob(uPts[i].cwiseProduct(scale));
			}
			tri=std::get<1>(uSphTri); // this makes a copy, but we need out own for capsules
		}
		if(capsule){
			#ifdef WOO_VTK
				int subdiv=max(4.,ceil(M_PI/(acos(1-tol/capsule->radius))));
				std::tie(pts,tri)=VtkExport::triangulateCapsule(static_pointer_cast<Capsule>(p->shape),subdiv);
			#else
				throw std::runtime_error("Triangulation of capsules is (for internal and entirely fixable reasons) only available when compiled with the 'vtk' features.");
			#endif
		}
		// do not write out directly, store first for later
		ppts.push_back(pts);
		ttri.push_back(tri);
		LOG_TRACE("#"<<p->id<<" triangulated: "<<tri.size()<<","<<pts.size()<<" faces,vertices.");

		if(scene->isPeriodic){
			// make sure we have aabb, in skewed coords and such
			if(!p->shape->bound){
				// this is a bit ugly, but should do the trick; otherwise we would recompute all that ourselves here
				if(sphere) Bo1_Sphere_Aabb().go(p->shape);
				else if(ellipsoid) Bo1_Ellipsoid_Aabb().go(p->shape);
				else if(capsule) Bo1_Capsule_Aabb().go(p->shape);
			}
			assert(p->shape->bound);
			const AlignedBox3r& box(p->shape->bound->box);
			AlignedBox3r cell(Vector3r::Zero(),scene->cell->getSize()); // possibly in skewed coords
			// central offset
			Vector3i off0;
			scene->cell->canonicalizePt(p->shape->nodes[0]->pos,off0); // computes off0
			Vector3i off; // offset from the original cell
			//cerr<<"#"<<p->id<<" at "<<p->shape->nodes[0]->pos.transpose()<<", off0="<<off0<<endl;
			for(off[0]=off0[0]-1; off[0]<=off0[0]+1; off[0]++) for(off[1]=off0[1]-1; off[1]<=off0[1]+1; off[1]++) for(off[2]=off0[2]-1; off[2]<=off0[2]+1; off[2]++){
				Vector3r dx=scene->cell->intrShiftPos(off);
				//cerr<<"  off="<<off.transpose()<<", dx="<<dx.transpose()<<endl;
				AlignedBox3r boxOff(box); boxOff.translate(dx);
				//cerr<<"  boxOff="<<boxOff.min()<<";"<<boxOff.max()<<" | cell="<<cell.min()<<";"<<cell.max()<<endl;
				if(boxOff.intersection(cell).isEmpty()) continue;
				// copy the entire triangulation, offset by dx
				vector<Vector3r> pts2(pts); for(auto& p: pts2) p+=dx;
				vector<Vector3i> tri2(tri); // same topology
				ppts.push_back(pts2);
				ttri.push_back(tri2);
				LOG_TRACE("  offset "<<off.transpose()<<": #"<<p->id<<": "<<tri2.size()<<","<<pts2.size()<<" faces,vertices.");
			}
		}
	}

	if(!merge){
		LOG_DEBUG("Will export (unmerged) "<<ppts.size()<<" particles to STL.");
		stl<<"solid "<<solid<<"\n";
		for(size_t i=0; i<ppts.size(); i++){
			const auto& pts(ppts[i]);
			const auto& tri(ttri[i]);
			LOG_TRACE("Exporting "<<i<<" with "<<tri.size()<<" faces.");
			for(const Vector3i& t: tri){
				Vector3r pp[]={pts[t[0]],pts[t[1]],pts[t[2]]};
				// skip triangles which are entirely out of the canonical periodic cell
				if(scene->isPeriodic && clipCell && (!scene->cell->isCanonical(pp[0]) && !scene->cell->isCanonical(pp[1]) && !scene->cell->isCanonical(pp[2]))) continue;
				numTri++;
				Vector3r n=(pp[1]-pp[0]).cross(pp[2]-pp[1]).normalized();
				stl<<"  facet normal "<<n.x()<<" "<<n.y()<<" "<<n.z()<<"\n";
				stl<<"    outer loop\n";
				for(auto p: {pp[0],pp[1],pp[2]}){
					stl<<"      vertex "<<p[0]<<" "<<p[1]<<" "<<p[2]<<"\n";
				}
				stl<<"    endloop\n";
				stl<<"  endfacet\n";
			}
		}
		stl<<"endsolid "<<solid<<"\n";
		stl.close();
		return numTri;
	}

#if WOO_GTS
	/*****
	Convert all triangulation to GTS surfaces, find their distances, isolate connected components,
	merge these components incrementally and write to STL
	*****/

	// total number of points
	const size_t N(ppts.size());
	// bounds for collision detection
	struct Bound{
		Bound(Real _coord, int _id, bool _isMin): coord(_coord), id(_id), isMin(_isMin){};
		Bound(): coord(NaN), id(-1), isMin(false){}; // just for allocation
		Real coord;
		int id;
		bool isMin;
		bool operator<(const Bound& b) const { return coord<b.coord; }
	};
	vector<Bound> bounds[3]={vector<Bound>(2*N),vector<Bound>(2*N),vector<Bound>(2*N)};
	/* construct GTS surface objects; all objects must be deleted explicitly! */
	vector<GtsSurface*> ssurf(N);
	vector<vector<GtsVertex*>> vvert(N);
	vector<vector<GtsEdge*>> eedge(N);
	vector<AlignedBox3r> boxes(N);
	for(size_t i=0; i<N; i++){
		LOG_TRACE("** Creating GTS surface for #"<<i<<", with "<<ttri[i].size()<<" faces, "<<ppts[i].size()<<" vertices.");
		AlignedBox3r box;
		// new surface object
		ssurf[i]=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class());
		// copy over all vertices
		vvert[i].reserve(ppts[i].size());
		eedge[i].reserve(size_t(1.5*ttri[i].size())); // each triangle consumes 1.5 edges, for closed surfs
		for(size_t v=0; v<ppts[i].size(); v++){
			vvert[i].push_back(gts_vertex_new(gts_vertex_class(),ppts[i][v][0],ppts[i][v][1],ppts[i][v][2]));
			box.extend(ppts[i][v]);
		}
		// create faces, and create edges on the fly as needed
		std::map<std::pair<int,int>,int> edgeIndices;
		for(size_t t=0; t<ttri[i].size(); t++){
			//const Vector3i& t(ttri[i][t]);
			//LOG_TRACE("Face with vertices "<<ttri[i][t][0]<<","<<ttri[i][t][1]<<","<<ttri[i][t][2]);
			Vector3i eIxs;
			for(int a:{0,1,2}){
				int A(ttri[i][t][a]), B(ttri[i][t][(a+1)%3]);
				auto AB=std::make_pair(min(A,B),max(A,B));
				auto ABI=edgeIndices.find(AB);
				if(ABI==edgeIndices.end()){ // this edge not created yet
					edgeIndices[AB]=eedge[i].size(); // last index 
					eIxs[a]=eedge[i].size();
					//LOG_TRACE("  New edge #"<<eIxs[a]<<": "<<A<<"--"<<B<<" (length "<<(ppts[i][A]-ppts[i][B]).norm()<<")");
					eedge[i].push_back(gts_edge_new(gts_edge_class(),vvert[i][A],vvert[i][B]));
				} else {
					eIxs[a]=ABI->second;
					//LOG_TRACE("  Found edge #"<<ABI->second<<" for "<<A<<"--"<<B);
				}
			}
			//LOG_TRACE("  New face: edges "<<eIxs[0]<<"--"<<eIxs[1]<<"--"<<eIxs[2]);
			GtsFace* face=gts_face_new(gts_face_class(),eedge[i][eIxs[0]],eedge[i][eIxs[1]],eedge[i][eIxs[2]]);
			gts_surface_add_face(ssurf[i],face);
		}
		// make sure the surface is OK
		if(!gts_surface_is_orientable(ssurf[i])) LOG_ERROR("Surface of #"+to_string(iid[i])+" is not orientable (expect troubles).");
		if(!gts_surface_is_closed(ssurf[i])) LOG_ERROR("Surface of #"+to_string(iid[i])+" is not closed (expect troubles).");
		assert(!gts_surface_is_self_intersecting(ssurf[i]));
		// copy bounds
		LOG_TRACE("Setting bounds of surf #"<<i);
		boxes[i]=box;
		for(int ax:{0,1,2}){
			bounds[ax][2*i+0]=Bound(box.min()[ax],/*id*/i,/*isMin*/true);
			bounds[ax][2*i+1]=Bound(box.max()[ax],/*id*/i,/*isMin*/false);
		}
	}

	/*
	broad-phase collision detection between GTS surfaces
	only those will be probed with exact algorithms below and merged if needed
	*/
	for(int ax:{0,1,2}) std::sort(bounds[ax].begin(),bounds[ax].end());
	vector<Bound>& bb(bounds[0]); // run the search along x-axis, does not matter really
	std::list<std::pair<int,int>> int0; // broad-phase intersections
	for(size_t i=0; i<2*N; i++){
		if(!bb[i].isMin) continue; // only start with lower bound
		// go up to the upper bound, but handle overflow safely (no idea why it would happen here) as well
		for(size_t j=i+1; j<2*N && bb[j].id!=bb[i].id; j++){
			if(bb[j].isMin) continue; // this is handled by symmetry
			#if EIGEN_VERSION_AT_LEAST(3,2,5)
				if(!boxes[bb[i].id].intersects(boxes[bb[j].id])) continue; // no intersection along all axes
			#else
				// old, less elegant
				if(boxes[bb[i].id].intersection(boxes[bb[j].id]).isEmpty()) continue; 
			#endif
			int0.push_back(std::make_pair(min(bb[i].id,bb[j].id),max(bb[i].id,bb[j].id)));
			LOG_TRACE("Broad-phase collision "<<int0.back().first<<"+"<<int0.back().second);
		}
	}

	/*
	narrow-phase collision detection between GTS surface
	this must be done via gts_surface_inter_new, since gts_surface_distance always succeeds
	*/
	std::list<std::pair<int,int>> int1;
	for(const std::pair<int,int> ij: int0){
		LOG_TRACE("Testing narrow-phase collision "<<ij.first<<"+"<<ij.second);
		#if 0
			GtsRange gr1, gr2;
			gts_surface_distance(ssurf[ij.first],ssurf[ij.second],/*delta ??*/(gfloat).2,&gr1,&gr2);
			if(gr1.min>0 && gr2.min>0) continue;
			LOG_TRACE("  GTS reports collision "<<ij.first<<"+"<<ij.second<<" (min. distances "<<gr1.min<<", "<<gr2.min);
		#else
			GtsSurface *s1(ssurf[ij.first]), *s2(ssurf[ij.second]);
			GNode* t1=gts_bb_tree_surface(s1);
			GNode* t2=gts_bb_tree_surface(s2);
			GtsSurfaceInter* I=gts_surface_inter_new(gts_surface_inter_class(),s1,s2,t1,t2,/*is_open_1*/false,/*is_open_2*/false);
			GSList* l=gts_surface_intersection(s1,s2,t1,t2); // list of edges describing intersection
			int n1=g_slist_length(l);
			// extra check by looking at number of faces of the intersected surface
			#if 1
				GtsSurface* s12=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class());
				gts_surface_inter_boolean(I,s12,GTS_1_OUT_2);
				gts_surface_inter_boolean(I,s12,GTS_2_OUT_1);
				int n2=gts_surface_face_number(s12);
				gts_object_destroy(GTS_OBJECT(s12));
			#endif
			gts_bb_tree_destroy(t1,TRUE);
			gts_bb_tree_destroy(t2,TRUE);
			gts_object_destroy(GTS_OBJECT(I));
			g_slist_free(l);
			if(n1==0) continue;
			#if 1
				if(n2==0){ LOG_ERROR("n1==0 but n2=="<<n2<<" (no narrow-phase collision)"); continue; }
			#endif
			LOG_TRACE("  GTS reports collision "<<ij.first<<"+"<<ij.second<<" ("<<n<<" edges describe the intersection)");
		#endif
		int1.push_back(ij);
	}
	/*
	connected components on the graph: graph nodes are 0…(N-1), graph edges are in int1
	see http://stackoverflow.com/a/37195784/761090
	*/
	typedef boost::subgraph<boost::adjacency_list<boost::vecS,boost::vecS,boost::undirectedS,boost::property<boost::vertex_index_t,int>,boost::property<boost::edge_index_t,int>>> Graph;
	Graph graph(N);
	for(const auto& ij: int1) boost::add_edge(ij.first,ij.second,graph);
	vector<size_t> clusters(boost::num_vertices(graph));
	size_t numClusters=boost::connected_components(graph,clusters.data());
	for(size_t n=0; n<numClusters; n++){
		// beginning cluster #n
		// first, count how many surfaces are in this cluster; if 1, things are easier
		int numThisCluster=0; int cluster1st=-1;
		for(size_t i=0; i<N; i++){ if(clusters[i]!=n) continue; numThisCluster++; if(cluster1st<0) cluster1st=(int)i; }
		GtsSurface* clusterSurf=NULL;
		LOG_DEBUG("Cluster "<<n<<" has "<<numThisCluster<<" surfaces.");
		if(numThisCluster==1){
			clusterSurf=ssurf[cluster1st]; 
		} else {
			clusterSurf=ssurf[cluster1st]; // surface of the cluster itself
			LOG_TRACE("  Initial cluster surface from "<<cluster1st<<".");
			/* composed surface */
			for(size_t i=0; i<N; i++){
				if(clusters[i]!=n || ((int)i)==cluster1st) continue;
				LOG_TRACE("   Adding "<<i<<" to the cluster");
				// ssurf[i] now belongs to cluster #n
				// trees need to be rebuild every time anyway, since the merged surface keeps changing in every cycle
				//if(gts_surface_face_number(clusterSurf)==0) LOG_ERROR("clusterSurf has 0 faces.");
				//if(gts_surface_face_number(ssurf[i])==0) LOG_ERROR("Surface #"<<i<<" has 0 faces.");
				GNode* t1=gts_bb_tree_surface(clusterSurf);
				GNode* t2=gts_bb_tree_surface(ssurf[i]);
				GtsSurfaceInter* I=gts_surface_inter_new(gts_surface_inter_class(),clusterSurf,ssurf[i],t1,t2,/*is_open_1*/false,/*is_open_2*/false);
				GtsSurface* merged=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class());
				gts_surface_inter_boolean(I,merged,GTS_1_OUT_2);
				gts_surface_inter_boolean(I,merged,GTS_2_OUT_1);
				gts_object_destroy(GTS_OBJECT(I));
				gts_bb_tree_destroy(t1,TRUE);
				gts_bb_tree_destroy(t2,TRUE);
				if(gts_surface_face_number(merged)==0){
					LOG_ERROR("Cluster #"<<n<<": 0 faces after fusing #"<<i<<" (why?), adding #"<<i<<" separately!");
					// this will cause an extra 1-particle cluster to be created
					clusters[i]=numClusters;
					numClusters+=1;
				} else {
					// not from global vectors (cleanup at the end), explicit delete!
					if(clusterSurf!=ssurf[cluster1st]) gts_object_destroy(GTS_OBJECT(clusterSurf));
					clusterSurf=merged;
				}
			}
		}
		#if 0
			LOG_TRACE("  GTS surface cleanups...");
	 		pygts_vertex_cleanup(clusterSurf,.1*tol); // cleanup 10× smaller than tolerance
		   pygts_edge_cleanup(clusterSurf);
	      pygts_face_cleanup(clusterSurf);
		#endif
		LOG_TRACE("  STL: cluster "<<n<<" output");
		stl<<"solid "<<solid<<"_"<<n<<"\n";
		/* output cluster to STL here */
		_gts_face_to_stl_data data(stl,scene,clipCell,numTri);
		gts_surface_foreach_face(clusterSurf,(GtsFunc)_gts_face_to_stl,(gpointer)&data);
		stl<<"endsolid\n";
		if(clusterSurf!=ssurf[cluster1st]) gts_object_destroy(GTS_OBJECT(clusterSurf));
	}
	// this deallocates also edges and vertices
	for(size_t i=0; i<ssurf.size(); i++) gts_object_destroy(GTS_OBJECT(ssurf[i]));
	return numTri;
#endif /* WOO_GTS */
}
Beispiel #4
0
void MimRec::calc_distance()
{
	int countPoints = -1;
	for (int i = 0; i < numberOfFFP; i++)
	{
		cv::Vec2f pts1(getX(i), getY(i));
		float ptsYAxis1 = getY(i);
		for (int j = i; j < numberOfFFP; j++)
		{
			countPoints++;
			cv::Vec2f pts2(getX(j), getY(j));

			current_distances[countPoints] = cv::norm(pts1, pts2);

			float ptsYAxis2 = getY(j);
			distancesYAxis[countPoints] = ptsYAxis1 - ptsYAxis2;
		}

	}

	//special values for Lid Tightener
	cv::Vec2f middlePointrightEye((getX(47) + getX(44)) / 2, (getY(47) + getY(44)) / 2);
	cv::Vec2f ptlidright(getX(45), getY(45));
	current_distances[0] = cv::norm(middlePointrightEye, ptlidright);

	cv::Vec2f middlePointLeftEye((getX(22) + getX(19)) / 2, (getY(22) + getY(19)) / 2);
	cv::Vec2f ptlidLeft(getX(20), getY(20));
	current_distances[1] = cv::norm(middlePointLeftEye, ptlidLeft);


	cv::Vec2f ptLowerlidright(getX(46), getY(46));
	current_distances[5461] = cv::norm(middlePointrightEye, ptLowerlidright);

	cv::Vec2f ptLowerlidLeft(getX(21), getY(21));
	current_distances[5462] = cv::norm(middlePointLeftEye, ptLowerlidLeft);


	//angle between 7-8-30 for Lip Corner Depressor
	current_distances[5463] =
		acos((pow(current_distances[826], 2) + pow(current_distances[708], 2) - pow(current_distances[730], 2)) / (2 * abs(current_distances[826]) * abs(current_distances[708])));
	current_distances[5463] *= 180.f / 3.14f;

	current_distances[5464] =
		acos((pow(current_distances[851], 2) + pow(current_distances[708], 2) - pow(current_distances[755], 2)) / (2 * abs(current_distances[708]) * abs(current_distances[851])));
	current_distances[5464] *= 180.f / 3.14f;
	

	//angle between 6-5-30 for Lip Corner Puller
	current_distances[5465] =
		acos((pow(current_distances[535], 2) + pow(current_distances[511], 2) - pow(current_distances[633], 2)) / (2 * abs(current_distances[535]) * abs(current_distances[511])));
	current_distances[5465] *= 180.f / 3.14f;

	current_distances[5466] =
		acos((pow(current_distances[560], 2) + pow(current_distances[511], 2) - pow(current_distances[658], 2)) / (2 * abs(current_distances[560]) * abs(current_distances[511])));
	current_distances[5466] *= 180.f / 3.14f;

	//angle between 6 -55 -30 for funneler
	current_distances[5467] =
		acos((pow(current_distances[633], 2) + pow(current_distances[658], 2) - pow(current_distances[2710], 2)) / (2 * abs(current_distances[633]) * abs(current_distances[658])));
	current_distances[5467] *= 180 / 3.14f;
	
	//angle between 22-19-16 for Inner Brow Raiser left
	current_distances[5468] =
		acos((pow(current_distances[1808], 2) + pow(current_distances[1547], 2) - pow(current_distances[1550], 2)) / (2 * abs(current_distances[1808]) * abs(current_distances[1547])));
	current_distances[5468] *= 180.f / 3.14f;
	//angle between 41-44-47 für Inner Brow Raiser right
	current_distances[5469] =
		acos((pow(current_distances[3447], 2) + pow(current_distances[3633], 2) - pow(current_distances[3450], 2)) / (2 * abs(current_distances[3447]) * abs(current_distances[3633])));
	current_distances[5469] *= 180.f / 3.14f;

	//angle between 22-17-19 for Outer Brow Raiser left
	current_distances[5470] =
		acos((pow(current_distances[1637], 2) + pow(current_distances[1808], 2) - pow(current_distances[1634], 2)) / (2 * abs(current_distances[1637]) * abs(current_distances[1808])));
	current_distances[5470] *= 180.f / 3.14f;
	//angle between 44-42-47 für Outer Brow Raiser right
	current_distances[5471] =
		acos((pow(current_distances[3633], 2) + pow(current_distances[3512], 2) - pow(current_distances[3509], 2)) / (2 * abs(current_distances[3633]) * abs(current_distances[3512])));
	current_distances[5471] *= 180.f / 3.14f;

	//cheek raiser
	cv::Vec2f cheek1(getX(26), getY(26));
	current_distances[5472] = cv::norm(middlePointrightEye, cheek1);

	//lip stretcher
	current_distances[5473] = abs(getX(55) - getX(30));

}
Beispiel #5
0
long VisualOdometry::run(const cv::Mat& left_frame, const cv::Mat& right_frame) {

	// print frame info
	cout << "start>> landmarks:" << landmarks.size()
			<< " trials:" << trials.size() << endl;

	// run time counting
	long t1 = cv::getTickCount();

	// state machine
	switch (state) {

	case VO_START:
		graph.addFirstPose(frameCount);
		featureAssoc->initTrials(left_frame, right_frame, frameCount, trials);
		featureAssoc->visualizePair(trials);
		state = VO_BOOT;
		break;

	case VO_BOOT:
		featureAssoc->processImage(left_frame, right_frame, frameCount, landmarks, trials, unmatched);
		if (trials.size() < trialThre+200) {
			cout << "--confirm all initial trials--" << endl;
			confirmTrials();
			trialState = false;
			state = VO_NORMAL;
		}
		graph.advancePoseTrivial(frameCount);
		break;

	case VO_NORMAL:
		// abnormal case
		if(landmarks.empty()) {
			cerr<<"!!!!!!!!!!!!ERROR::NO LANDMARKS!!!!!!!!!!!"<<endl;
			graph.advancePoseTrivial(frameCount);
			break;
		}

		// reset inlier
		for (auto it = landmarks.begin(); it != landmarks.end(); it++) {
			it->second.setInlier(false);
		}

		// run tracker
		featureAssoc->processImage(left_frame, right_frame, frameCount, landmarks, trials, unmatched);
		// print frame info
		cout << "track>> landmarks:" << landmarks.size()
					<< " trials:" << trials.size() << endl;

		// confirm all trails
		if (trialState && trials.size() < trialThre) {
			cout << "--confirm all trials--" << endl;
			confirmTrials();
			trialState = false;
		}

		// prepare points
		cv::Mat pts1(2, landmarks.size(), cv::DataType<float>::type),
				pts2(2, landmarks.size(), cv::DataType<float>::type);
		vector<cv::Point2f> imgPts;
		int colcount = 0;
		for (auto it = landmarks.begin(); it != landmarks.end(); it++) {
			cv::KeyPoint k1, k2;
			it->second.prevPointPair(k1, k2);
			pts1.at<float>(0, colcount) = k1.pt.x;
			pts1.at<float>(1, colcount) = k1.pt.y;
			pts2.at<float>(0, colcount) = k2.pt.x;
			pts2.at<float>(1, colcount) = k2.pt.y;
			imgPts.push_back(it->second.curLeftPoint().pt);
			colcount++;
		}

		// triangulate 3D points
		cv::Mat pts3D;
		triangulate(pts1, pts2, pts3D);

		// prepare 3D points
		cv::Mat R, Rvec, Tvec, inliers;
		vector<cv::Point3f> objPts;
		for (int c = 0; c < pts3D.cols; c++) {
			objPts.push_back(cv::Point3f(pts3D.at<float>(0, c),
					pts3D.at<float>(1, c),
					pts3D.at<float>(2, c)));
		}

		// incremental 3D-2D VO
		cv::solvePnPRansac(objPts, imgPts, camera.intrinsic,
				cv::noArray(), Rvec, Tvec, false, 3000, 1.0, 0.99, inliers, cv::SOLVEPNP_P3P);
		cv::Rodrigues(Rvec, R);

		//cout << R << Tvec << endl;
		//cout << inliers << endl;
		cout << "P3P points:" << imgPts.size() << " inliers:" << inliers.rows << endl;

		//set inliers
		int j = 0, count = 0;
		if (inliers.rows > 0) {
			for (auto it = landmarks.begin(); it != landmarks.end(); it++) {
				if (count++ == inliers.at<int>(j, 0)) {
					it->second.setInlier(true);
					if (++j >= inliers.rows)
						break;
				}
			}
		} else {
			cerr<<"!!!!!!!!!!!!WARNING::PNP FAILED!!!!!!!!!!!"<<endl;
			R = cv::Mat::eye(3, 3, cv::DataType<float>::type);
			Tvec = cv::Mat::zeros(3, 1, cv::DataType<float>::type);
			cv::randn(Tvec,cv::Scalar::all(0),cv::Scalar::all(0.001));
		}

		// add stereo factors
		for (auto it = landmarks.begin(); it != landmarks.end(); it++) {
			if (it->second.isInlier()) {
				cv::KeyPoint p1, p2;
				it->second.curPointPair(p1, p2);
				graph.addStereo(frameCount, it->first, p1.pt, p2.pt);
			}
		}

		// add initial pose estimation
		allR.push_back(R);
		allT.push_back(Tvec);
		graph.advancePose(frameCount, R, Tvec);

//--------loop constraint hack-------------
#if LOOP
		if(frameCount == 3700)
			graph.addLoopConstraint(0,3700);
#endif
//--------loop constraint hack--------------

		// run bundle adjustment
		long u1 = cv::getTickCount();
		//graph.batchUpdate();
		graph.increUpdate();
		long u2 = cv::getTickCount();
		cout << "optimization:" << float(u2 - u1) / cv::getTickFrequency() << endl;
		updateLandmark();

		if (!trialState && landmarks.size() < landmarkThre) {
			cout << "--init new trails--" << endl;
			featureAssoc->refreshTrials(left_frame, right_frame, frameCount, unmatched, trials);
			featureAssoc->visualizePair(trials);
			trialState = true;

			if (trials.size() < directAddThre || landmarks.size() < lowlandThre) {
				//just add them don't wait
				cerr<<"!!!!!!!!!!!!WARNING::LOW LANDMARKS!!!!!!!!!!!"<<endl;
				confirmTrials();
				trialState = false;
			}
		}
		break;
	}

	// advance frame count number
	frameCount++;

	// count runing time
	long t2 = cv::getTickCount();

	// print info
	cout << "end>> landmarks:" << landmarks.size()
			<< " trials:" << trials.size() << endl;
	cout << "time:" << float(t2 - t1) / cv::getTickFrequency() << endl;

	// return running time
	return t2 - t1;
}