matf Triangulate(const matf & P1, const matf & P2, const TrackedPoint & p, bool full) { matf A; if (full) A = matf(6, 4); else A = matf(4,4); for (int i = 0; i < 4; ++i) { A(0, i) = p.x1 * P1(2, i) - P1(0, i); A(1, i) = p.y1 * P1(2, i) - P1(1, i); A(2, i) = p.x2 * P2(2, i) - P2(0, i); A(3, i) = p.y2 * P2(2, i) - P2(1, i); // the two following equations could be removed in most of the cases. // however, sometimes it induces errors if some (which ones?) coefficients are // too close to 0 if (full) { A(4, i) = p.x1 * P1(1, i) - p.y1 * P1(0, i); A(5, i) = p.x2 * P2(1, i) - p.y2 * P2(0, i); } } SVD svd(A, SVD::MODIFY_A); matf p3d(3,1); for (int i = 0; i < 3; ++i) p3d(i,0) = svd.vt.at<float>(3,i) / svd.vt.at<float>(3,3); return p3d; }
Box3D b3d(float x, float y, float z, float w, float d, float h) { return (Box3D){ p3d(x, y, z), s3d(w, d, h) }; }
void AddVertex (float x, float y) { PerspectiveOutlet2D::AddVertex (x, y); csVector4 p (x/hw-1, y/hh-1, 1.0f, 1.0f); csVector4 p3d (cam->GetInvProjectionMatrix() * p); p3d *= p3d.w; dest3D.AddVertex (csVector3 (p3d[0], p3d[1], p3d[2])); }
matf Triangulate(const matf & P1, const matf & P2, const matf & p) { matf A(6, 4); for (int i = 0; i < 4; ++i) { A(0, i) = p(0, 0) * P1(2, i) - P1(0, i); A(1, i) = p(1, 0) * P1(2, i) - P1(1, i); A(2, i) = p(2, 0) * P2(2, i) - P2(0, i); A(3, i) = p(3, 0) * P2(2, i) - P2(1, i); // the two following equations could be removed in most of the cases. // however, sometimes it induces errors if some (which ones?) coefficients are // too close to 0 A(4, i) = p(0, 0) * P1(1, i) - p(1, 0) * P1(0, i); A(5, i) = p(2, 0) * P2(1, i) - p(3, 0) * P2(0, i); } SVD svd(A, SVD::MODIFY_A); matf p3d(3,1); for (int i = 0; i < 3; ++i) p3d(i,0) = svd.vt.at<float>(3,i) / svd.vt.at<float>(3,3); return p3d; }
/* Returns the Y axis inverted */ Matx31d KinectSensor::pointBackproject(const Matx31d& point2D) const { XnPoint3D realPoint; XnPoint3D camPoint; camPoint.X = point2D(0); camPoint.Y = point2D(1); camPoint.Z = point2D(2); depthNode.ConvertProjectiveToRealWorld(1, &camPoint, &realPoint); Matx31d out (realPoint.X, realPoint.Y, realPoint.Z); //return out; Matx31d p3d; p3d(0) = (point2D(0) - ox)*pSize*point2D(2)/focalLength; p3d(1) = (point2D(1) - oy)*pSize*point2D(2)/focalLength; p3d(2) = point2D(2); return p3d; }
BOOST_FIXTURE_TEST_CASE(test_ostream_operator, F) { egen::Point<double> p2d(2.2, 1.0); egen::Point<double> p3d(3.0, 2.0, 1.0); std::ostringstream actual2d; actual2d << p2d; BOOST_CHECK_EQUAL("(2.2, 1)", actual2d.str()); std::ostringstream actual3d; actual3d << p3d; BOOST_CHECK_EQUAL("(3, 2, 1)", actual3d.str()); }
void Model::load_new_xml(const std::string path) { pugi::xml_document doc; pugi::xml_parse_result result = doc.load_file(path.c_str()); if (!result) { std::cout << "ERRROR opening model file!!"; return ; } pugi::xml_node pts_node = doc.child("Model").child("Points"); if (pts_node.first_child()) f_type = pts_node.first_child().attribute("desc_type").as_string(); else return; for (pugi::xml_node pt_node = pts_node.first_child(); pt_node; pt_node = pt_node.next_sibling()) { Mat single_desc = Mat::zeros(1, get_descriptor_size(), CV_32FC1); stringstream ss; ss.str(pt_node.attribute("p3d").as_string()); float x, y, z; ss >> x >> y >> z; ss.clear(); cv::Point3f p3d(x, y, z); list_points3d_in_.push_back(p3d); ss.str(pt_node.attribute("p2d_prop").as_string()); cv::KeyPoint kp; ss >> kp.pt.x >> kp.pt.y >> kp.octave >> kp.angle >> kp.response >> kp.size; ss.clear(); list_keypoints_.push_back(kp); ss.str(pt_node.attribute("desc").as_string()); float d; for(int it = 0; it < get_descriptor_size(); it ++) { ss >> d; //cout <<it << " " << ss.eof() << " " << d << endl; single_desc.at<float> (0, it) = d; } ss.clear(); descriptors_.push_back(single_desc); } id = path; //std::cout << list_points3d_in_.size() << endl; //std::cout << descriptors_; }
DLL_HEADER int Ngx_Mesh :: FindElementOfPoint <3> (double * p, double * lami, bool build_searchtree, int * const indices, int numind) const { Array<int> dummy(numind); for (int i = 0; i < numind; i++) dummy[i] = indices[i]+1; Point<3> p3d(p[0], p[1], p[2]); int ind = mesh->GetElementOfPoint(p3d, lami, &dummy, build_searchtree); return ind-1; }
std::pair<bool, Imath::V3d> GLWidget::mapToSphere(Imath::V2d p2d) { Imath::V3d p3d(0.0, 0.0, 0.0); if ( ((p2d.x >= 0) & (p2d.x <= width()) & (p2d.y >= 0) & (p2d.y <= height())) ) { float x = (p2d.x - 0.5 * width()) / width(); float y = (0.5 * height() - p2d.y) / height(); p3d[0] = x; p3d[1] = y; float z2 = (2.0 * 0.5 * 0.5 - x * x - y * y); p3d[2] = sqrt(std::max(z2, (float)0)); Imath::V3d n = p3d.normalize(); p3d = p3d / n; return std::make_pair(true, p3d); } else { return std::make_pair(false, p3d); } }
void VoxelGrid<PointSourceType>::scatterPointsToVoxelGrid() { for (int pid = 0; pid < source_cloud_->points.size(); pid++) { int vid = voxelId(source_cloud_->points[pid]); PointSourceType p = source_cloud_->points[pid]; Eigen::Vector3d p3d(p.x, p.y, p.z); if (points_id_[vid].size() == 0) { centroid_[vid].setZero(); covariance_[vid].setIdentity(); icovariance_[vid].setZero(); points_per_voxel_[vid] = 0; } centroid_[vid] += p3d; covariance_[vid] += p3d * p3d.transpose(); points_id_[vid].push_back(pid); points_per_voxel_[vid]++; } }
void Model::load_new_desc(const std::string path) { f_type = "SIFT"; std::fstream infile(path.c_str()); if (!infile.is_open()) { std::cout << "ERRROR opening model file!!"; return ; } int nop, desc_size, point_size; infile >> nop; infile >> point_size >> desc_size; descriptors_ = Mat(nop, desc_size, CV_32FC1); for (int i = 0; i < nop; i++) { float x, y, z; infile >> x >> y >> z; cv::Point3f p3d(x, y, z); list_points3d_in_.push_back(p3d); cv::KeyPoint kp; infile >> kp.pt.x >> kp.pt.y >> kp.octave >> kp.angle >> kp.response >> kp.size; list_keypoints_.push_back(kp); for (int j = 0; j < desc_size; j++) { infile >> descriptors_.at<float>(i, j); } } id = path; }
void transformPoint( Point& point, Matrix3x3* transform ) { Vector2D p = point.position; Vector3D p3d ( p.x, p.y, 1.0 ); p3d = (*transform) * p3d; point.position = Vector2D(p3d.x / p3d.z, p3d.y / p3d.z); }
Point3D p3dNeg(Point3D p) { return p3d(-p.x, -p.y, -p.z); }
Point3D p3dSub(Point3D p1, Point3D p2) { return p3d(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z); }
Point3D p3dAdd(Point3D p1, Point3D p2) { return p3d(p1.x + p2.x, p1.y + p2.y, p1.z + p2.z); }
void DeformationGraph::GenerateDensePointSet(std::list<PointWithID>& pointcoll, double delta) { #ifdef DEBUGTRACE meshtalent::DebugTrace dt("./sample.log"); #endif assert(pointcoll.size() == 0); typedef DeformableMesh3d::InterMesh InterMesh; InterMesh* pMesh = dmesh.pMesh; int vertexnum = pMesh->n_vertices(); //int edgenum = pMesh->n_edges(); //int facetnum = pMesh->n_faces(); // sample points on vertices. InterMesh::VertexIter v_it, v_end(pMesh->vertices_end()); for (v_it = pMesh->vertices_begin(); v_it != v_end; ++v_it) { InterMesh::Point& p = pMesh->point(v_it); P3d p3d(p[0], p[1], p[2]); pointcoll.push_back(PointWithID(p3d, PointWithID::VERTEX, v_it.handle().idx())); } #ifdef DEBUGTRACE dt.Trace("There %d vertex_points sampled.\n", vertexnum); #endif // sample points on edges. int epcount = 0; InterMesh::EdgeIter e_it, e_end(pMesh->edges_end()); for (e_it = pMesh->edges_begin(); e_it != e_end; ++e_it) { InterMesh::EdgeHandle eh = pMesh->handle(*e_it); InterMesh::HalfedgeHandle heh = pMesh->halfedge_handle(eh, 0); // always select the first edge, but if it's a boundary?? InterMesh::VertexHandle phfrom = pMesh->from_vertex_handle(heh); InterMesh::VertexHandle phto = pMesh->to_vertex_handle(heh); InterMesh::Point pf = pMesh->point(phfrom); InterMesh::Point pt = pMesh->point(phto); P3d p3dfrom(pf[0], pf[1], pf[2]); P3d p3dto(pt[0], pt[1], pt[2]); double dist = (p3dto - p3dfrom).mag(); //compute delta vector and sample on this edge. V3d deltaV = (p3dto - p3dfrom) * delta; int times = static_cast<int>(dist / delta) - 1; P3d p3dnow = p3dfrom; for (int j = 0; j < times; ++j) { p3dnow += deltaV; pointcoll.push_back(PointWithID(p3dnow, PointWithID::EDGE, e_it.handle().idx())); } epcount += (times < 0 ? 0 : times); } #ifdef DEBUGTRACE dt.Trace("There are %d edge_points sampled.\n", epcount); #endif // sample points on facet. int fpcount = 0; InterMesh::FaceIter f_it, f_end(pMesh->faces_end()); for (f_it = pMesh->faces_begin(); f_it != f_end; ++f_it) { P3d trivertices[3]; //assert((*f_it).is_triangle()); InterMesh::FaceVertexIter fv_it(pMesh->fv_iter(f_it.handle())); int j = 0; for (; fv_it; ++fv_it, ++j) { InterMesh::Point& p = pMesh->point(fv_it); trivertices[j] = P3d(p[0], p[1], p[2]); } // compute num should be sampled. V3d v1 = trivertices[1] - trivertices[0]; V3d v2 = trivertices[2] - trivertices[0]; //double lenv1 = v1.mag(); //double lenv2 = v2.mag(); double area = (v1 % v2).mag() / 2; int samplenum = static_cast<int>(area / (delta * delta)); fpcount += samplenum; RandDoubleGenerator drand; drand.srand(time(NULL)); while (samplenum > 0) { double lamada1 = drand(0, 1); double lamada2 = drand(0, 1); if (lamada1 + lamada2 >= 1) { // judge if this point is not in triangle. continue; } P3d tp3d = trivertices[0] + (v1 * lamada1 + v2 * lamada2); pointcoll.push_back(PointWithID(tp3d, PointWithID::FACE, f_it.handle().idx())); --samplenum; } // end of while. } // end of for. #ifdef DEBUGTRACE dt.Trace("There are %d facet_points sampled.\n", fpcount); dt.Trace("There are %d points sampled totally.\n", pointcoll.size()); #endif }
vector<ofVec3f> findRectangle(const PCPtr& cloud, const pcl::ModelCoefficients& coefficients) { vector<Eigen::Vector3f> corners; //saveCloudAsFile("toproject.pcd",*cloud); pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients(coefficients)); // Project points onto the table plane PC projectedCloud = *cloud; //saveCloudAsFile("projected.pcd",projectedCloud); PCXYZ pto = cloud->at(1); float val = coefficients.values[0] * pto.x + coefficients.values[1] * pto.y + coefficients.values[2] * pto.z + coefficients.values[3]; if(abs(val) < 0.009) ;; // store the table top plane parameters Eigen::Vector3f planeNormal; planeNormal.x() = coeff->values[0]; planeNormal.y() = coeff->values[1]; planeNormal.z() = coeff->values[2]; // compute an orthogonal normal to the plane normal Eigen::Vector3f v = planeNormal.unitOrthogonal(); // take the cross product of the two normals to get // a thirds normal, on the plane Eigen::Vector3f u = planeNormal.cross(v); // project the 3D point onto a 2D plane std::vector<cv::Point2f> points; // choose a point on the plane Eigen::Vector3f p0(projectedCloud.points[0].x, projectedCloud.points[0].y, projectedCloud.points[0].z); for(unsigned int ii=0; ii<projectedCloud.points.size(); ii++) { Eigen::Vector3f p3d(projectedCloud.points[ii].x, projectedCloud.points[ii].y, projectedCloud.points[ii].z); // subtract all 3D points with a point in the plane // this will move the origin of the 3D coordinate system // onto the plane p3d = p3d - p0; cv::Point2f p2d; p2d.x = p3d.dot(u); p2d.y = p3d.dot(v); points.push_back(p2d); } cv::Mat pointsMat(points); cv::RotatedRect rrect = cv::minAreaRect(pointsMat); cv::Point2f rrPts[4]; rrect.points(rrPts); //store the table top bounding points in a vector for(unsigned int ii=0; ii<4; ii++) { Eigen::Vector3f pbbx(rrPts[ii].x*u + rrPts[ii].y*v + p0); corners.push_back(pbbx); } /*Eigen::Vector3f center(rrect.center.x*u + rrect.center.y*v + p0); corners.push_back(center); */ //Ver si se puede eliminar esto. vector<ofVec3f> vecCorners = projectPointsInPlane(corners,coefficients); //saveCloudAsFile("projectedrectangle.pcd",vecCorners); return vecCorners; }
int mitkBaseDataTest(int /*argc*/, char* /*argv*/[]) { MITK_TEST_BEGIN("BaseData") //Create a BaseData implementation MITK_INFO << "Creating a base data instance..."; mitk::BaseDataTestImplementation::Pointer baseDataImpl = mitk::BaseDataTestImplementation::New(); MITK_TEST_CONDITION_REQUIRED(baseDataImpl.IsNotNull(),"Testing instantiation"); MITK_TEST_CONDITION(baseDataImpl->IsInitialized(), "BaseDataTestImplementation is initialized"); MITK_TEST_CONDITION(baseDataImpl->IsEmpty(), "BaseDataTestImplementation is initialized and empty"); MITK_TEST_CONDITION(baseDataImpl->GetExternalReferenceCount()== baseDataImpl->GetReferenceCount(), "Checks external reference count!"); mitk::BaseDataTestImplementation::Pointer cloneBaseData = baseDataImpl->Clone(); MITK_TEST_CONDITION_REQUIRED(cloneBaseData.IsNotNull(),"Testing instantiation of base data clone"); MITK_TEST_CONDITION(cloneBaseData->IsInitialized(), "Clone of BaseDataTestImplementation is initialized"); MITK_TEST_CONDITION(cloneBaseData->IsEmpty(), "Clone of BaseDataTestImplementation is initialized and empty"); MITK_TEST_CONDITION(cloneBaseData->GetExternalReferenceCount()== cloneBaseData->GetReferenceCount(), "Checks external reference count of base data clone!"); MITK_INFO << "Testing setter and getter for geometries..."; //test method GetTimeSlicedGeometry() MITK_TEST_CONDITION(baseDataImpl->GetTimeSlicedGeometry(), "Testing creation of TimeSlicedGeometry"); mitk::TimeSlicedGeometry* geo = NULL; baseDataImpl->SetGeometry(geo); MITK_TEST_CONDITION(baseDataImpl->GetTimeSlicedGeometry() == NULL, "Reset Geometry"); mitk::TimeSlicedGeometry::Pointer geo2 = mitk::TimeSlicedGeometry::New(); baseDataImpl->SetGeometry(geo2); baseDataImpl->InitializeTimeSlicedGeometry(2); MITK_TEST_CONDITION(baseDataImpl->GetTimeSlicedGeometry() == geo2, "Correct Reinit of TimeslicedGeometry"); //test method GetGeometry(int timeStep) MITK_TEST_CONDITION(baseDataImpl->GetGeometry(1) != NULL, "... and single Geometries"); //test method Expand(unsigned int timeSteps) baseDataImpl->Expand(5); MITK_TEST_CONDITION(baseDataImpl->GetTimeSteps() == 5, "Expand the geometry to further time slices!"); //test method GetUpdatedGeometry(int timeStep); mitk::Geometry3D::Pointer geo3 = mitk::Geometry3D::New(); mitk::TimeSlicedGeometry::Pointer timeSlicedGeometry = baseDataImpl->GetTimeSlicedGeometry(); if (timeSlicedGeometry.IsNotNull() ) { timeSlicedGeometry->SetGeometry3D(geo3, 1); } MITK_TEST_CONDITION(baseDataImpl->GetUpdatedGeometry(1) == geo3, "Set Geometry for time step 1"); MITK_TEST_CONDITION(baseDataImpl->GetMTime()!= 0, "Check if modified time is set"); baseDataImpl->SetClonedGeometry(geo3, 1); float x[3]; x[0] = 2; x[1] = 4; x[2] = 6; mitk::Point3D p3d(x); baseDataImpl->SetOrigin(p3d); geo3->SetOrigin(p3d); MITK_TEST_CONDITION(baseDataImpl->GetGeometry(1)->GetOrigin() == geo3->GetOrigin(), "Testing Origin set"); cloneBaseData = baseDataImpl->Clone(); MITK_TEST_CONDITION(cloneBaseData->GetGeometry(1)->GetOrigin() == geo3->GetOrigin(), "Testing origin set in clone!"); MITK_TEST_CONDITION(!baseDataImpl->IsEmptyTimeStep(1), "Is not empty before clear()!"); baseDataImpl->Clear(); MITK_TEST_CONDITION(baseDataImpl->IsEmptyTimeStep(1), "...but afterwards!"); //test method Set-/GetProperty() baseDataImpl->SetProperty("property38", mitk::StringProperty::New("testproperty")); //baseDataImpl->SetProperty("visibility", mitk::BoolProperty::New()); MITK_TEST_CONDITION(baseDataImpl->GetProperty("property38")->GetValueAsString() == "testproperty","Check if base property is set correctly!"); cloneBaseData = baseDataImpl->Clone(); MITK_TEST_CONDITION(cloneBaseData->GetProperty("property38")->GetValueAsString() == "testproperty", "Testing origin set in clone!"); //test method Set-/GetPropertyList mitk::PropertyList::Pointer propertyList = mitk::PropertyList::New(); propertyList->SetFloatProperty("floatProperty1", 123.45); propertyList->SetBoolProperty("visibility",true); propertyList->SetStringProperty("nameXY","propertyName"); baseDataImpl->SetPropertyList(propertyList); bool value = false; MITK_TEST_CONDITION(baseDataImpl->GetPropertyList() == propertyList, "Check if base property list is set correctly!"); MITK_TEST_CONDITION(baseDataImpl->GetPropertyList()->GetBoolProperty("visibility", value) == true, "Check if base property is set correctly in the property list!"); //test method UpdateOutputInformation() baseDataImpl->UpdateOutputInformation(); MITK_TEST_CONDITION(baseDataImpl->GetUpdatedTimeSlicedGeometry() == geo2, "TimeSlicedGeometry update!"); //Test method CopyInformation() mitk::BaseDataTestImplementation::Pointer newBaseData = mitk::BaseDataTestImplementation::New(); newBaseData->CopyInformation(baseDataImpl); MITK_TEST_CONDITION_REQUIRED( newBaseData->GetTimeSlicedGeometry()->GetTimeSteps() == 5, "Check copying of of Basedata Data Object!"); MITK_TEST_END() }