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