C++ PointType类代码示例

AnyType TPTScriptInterface::tptS_create(std::deque<std::string> * words)
{
	//Arguments from stack
	AnyType createType = eval(words);
	PointType position = eval(words);

	Simulation * sim = m->GetSimulation();

	int type;
	if(createType.GetType() == TypeNumber)
		type = ((NumberType)createType).Value();
	else if(createType.GetType() == TypeString)
		type = GetParticleType(((StringType)createType).Value());
	else
		throw GeneralException("Invalid type");

	if(type == -1)
		throw GeneralException("Invalid particle type");

	ui::Point tempPoint = position.Value();
	if(tempPoint.X<0 || tempPoint.Y<0 || tempPoint.Y >= YRES || tempPoint.X >= XRES)
				throw GeneralException("Invalid position");

	int returnValue = sim->create_part(-1, tempPoint.X, tempPoint.Y, type);

	return NumberType(returnValue);
}

int mitk::PointSet::SearchPoint( Point3D point, float distance, int t  ) const
{
  if ( t >= (int)m_PointSetSeries.size() )
  {
    return -1;
  }
  
  // Out is the point which is checked to be the searched point
  PointType out;
  out.Fill( 0 );
  PointType indexPoint;

  this->GetGeometry( t )->WorldToIndex(point, indexPoint);

  // Searching the first point in the Set, that is +- distance far away fro
  // the given point
  unsigned int i;
  PointsContainer::Iterator it, end;
  end = m_PointSetSeries[t]->GetPoints()->End();
  int bestIndex = -1;
  distance = distance * distance;
  
  // To correct errors from converting index to world and world to index
  if (distance == 0.0)
  {
    distance = 0.000001;
  }

  ScalarType bestDist = distance;
  ScalarType dist, tmp;

  for ( it = m_PointSetSeries[t]->GetPoints()->Begin(), i = 0;
        it != end; 
        ++it, ++i )
  {
    bool ok = m_PointSetSeries[t]->GetPoints()
      ->GetElementIfIndexExists( it->Index(), &out );

    if ( !ok )
    {
      return -1;
    }
    else if ( indexPoint == out ) //if totally equal
    {
      return it->Index();
    }

    //distance calculation
    tmp = out[0] - indexPoint[0]; dist  = tmp * tmp;
    tmp = out[1] - indexPoint[1]; dist += tmp * tmp;
    tmp = out[2] - indexPoint[2]; dist += tmp * tmp;

    if ( dist < bestDist )
    {
      bestIndex = it->Index();
      bestDist  = dist;
    }
  }
  return bestIndex;
}

 bool contains(const PointType& p, bool strict = false) const
 {
     if (strict)
         return (p.array() > min.array()).all() && (p.array() < max.array()).all();
     else
         return (p.array() >= min.array()).all() && (p.array() <= max.array()).all();
 }

void Optimizer::eachCloudPair(CloudPair &pair)
{
    int cloud0 = pair.corresIdx.first;
    int cloud1 = pair.corresIdx.second;

    size_t matrix_size = m_pointClouds.size() * 6;

    TriContainer mat_elem;
    mat_elem.reserve(matrix_size * matrix_size / 5);
    SparseMatFiller filler(mat_elem);

    for (int i = 0; i < 6; ++i) {
        filler.add(i, i, 1.0);
    }

    Vec atb(matrix_size);
    Mat ata(matrix_size, matrix_size);
    atb.setZero(), ata.setZero();

    double score = 0.0;
    {
        //pcl::ScopeTime time("calculate LSE matrix");
    
#pragma unroll 8
        for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
            int point_p = pair.corresPointIdx[point_count].first;
            int point_q = pair.corresPointIdx[point_count].second;
            PointType P = m_pointClouds[cloud0]->points[point_p];
            PointType Q = m_pointClouds[cloud1]->points[point_q];

            Eigen::Vector3d p = P.getVector3fMap().cast<double>();
            Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
            Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();

            double b = -(p - q).dot(Np);
            score += b * b;
            Eigen::Matrix<double, 6, 1> A_p, A_q;
            A_p.block<3, 1>(0, 0) = p.cross(Np);
            A_p.block<3, 1>(3, 0) = Np;
            A_q.block<3, 1>(0, 0) = -q.cross(Np);
            A_q.block<3, 1>(3, 0) = -Np;
        
            filler.fill(cloud0, cloud1, A_p, A_q);
            atb.block<6, 1>(cloud0 * 6, 0) += A_p * b;
            atb.block<6, 1>(cloud1 * 6, 0) += A_q * b;
        }
        ata.setFromTriplets(mat_elem.begin(), mat_elem.end());
    }

    {
        //pcl::ScopeTime time("Fill sparse matrix");
        boost::mutex::scoped_lock lock(m_cloudPairMutex);
        //std::cout << "\tcurrent thread : " << boost::this_thread::get_id() << std::endl;
        //PCL_INFO("\tPair <%d, %d> alignment Score : %.6f\n", cloud0, cloud1, score);
        ATA += ata;
        ATb += atb;
        align_error += score;
    }
}

void
Tracer::ValueAt(const PointType& pt,
		double& dval) const
{
  InterpolatorType::PointType cpt;
  std::copy( pt.Begin(), pt.End(), cpt.Begin() );

  dval = imageInterpolator->Evaluate( cpt );
}

		// Use squared distances in order to avoid computing square roots
		double simpleDistance(PointType a, PointType b)
		{
			double sum = 0.0;
			for (int i = 0 ; i < dimensions ; i++)
			{
				double factor = a.cartesian(i) - b.cartesian(i);
				sum += factor * factor;
			}
			return sum;
		}

DynamicDSOOctree* DynamicDSOOctree::getChild(DeepSkyObject* const & _obj, const PointType& cellCenterPos)
{
    PointType objPos = _obj->getPosition();

    int child = 0;
    child     |= objPos.x() < cellCenterPos.x() ? 0 : XPos;
    child     |= objPos.y() < cellCenterPos.y() ? 0 : YPos;
    child     |= objPos.z() < cellCenterPos.z() ? 0 : ZPos;

    return _children[child];
}

void GraphNodeCtr::calculateNeigDis(Matrix3X & neigCoor,VecX& resDis)
{
	IndexType vtx_num = neigCoor.cols();
	PointType ori = neigCoor.col(0);
	PointType diff;
	diff.setZero();
	for (IndexType vtx_iter = 1; vtx_iter < vtx_num;vtx_iter ++)
	{
		diff = neigCoor.col(vtx_iter) - ori;
		resDis(vtx_iter,0) = diff.norm();
	}
}

bool TestClosestValueIndex()
{
  typedef itk::CovariantVector<float, 3> PointType;
  std::vector<PointType> vec;

  PointType a;
  a.Fill(1.1);
  vec.push_back(a);

  PointType b;
  b.Fill(2.1);
  vec.push_back(b);

  PointType c;
  c.Fill(3.1);
  vec.push_back(c);

  PointType query;
  query.Fill(1.2);

  unsigned int closestId = ITKHelpers::ClosestValueIndex(vec, query);

  std::cout << "Closest point to " << query << " is " << vec[closestId] << std::endl;

  return true;
}

  bool extract_symmetric_slice_3d::run(viennamesh::algorithm_handle &)
  {
    mesh_handle input_mesh = get_required_input<mesh_handle>("mesh");
    int geometric_dimension = viennagrid::geometric_dimension( input_mesh() );

    if (geometric_dimension != 3)
      return false;

    double tol = 1e-6;
    if (get_input<double>("tolerance").valid())
      tol = get_input<double>("tolerance")();

    typedef viennagrid::mesh                                                MeshType;
    typedef viennagrid::result_of::point<MeshType>::type                    PointType;


    PointType axis = get_required_input<point>("axis")();
    axis.normalize();

    int rotational_frequency = get_required_input<int>("rotational_frequency")();
    double angle = 2*M_PI/rotational_frequency;

    PointType N[2];
    N[0] = viennagrid::make_point(0,1,0);
    if ( std::abs(viennagrid::inner_prod(axis,N[0])) > 1.0-tol )
      N[0] = viennagrid::make_point(-1,0,0);
    N[0] -= axis * viennagrid::inner_prod(axis,N[0]);
    N[0].normalize();

    N[1] = -rotate(N[0], point(geometric_dimension), axis, angle);


    info(1) << "Using rotational frequency " << rotational_frequency << std::endl;
    info(1) << "Angle = " << angle << std::endl;
    info(1) << "Axis = " << axis << std::endl;
    info(1) << "Normal[0] = " << N[0] << std::endl;
    info(1) << "Normal[1] = " << N[1] << std::endl;


    mesh_handle tmp = cut( input_mesh(), N[0], tol );
    mesh_handle output_mesh = cut( tmp(), N[1], tol );

    set_output( "mesh", output_mesh );

    return true;
  }

// ------------------------------------------------------------------------
void computeBoundingBox(const MatrixType &points, const TestData::TransformType::Pointer &transform,
                        vtkBoundingBox &boundingBox)
{

    for(unsigned int i = 0; i < points.rows(); i++)
    {

        PointType p;
        for(unsigned int j = 0; j < 3; j++)
        {
            p[j] = points(i,j);
        }


        boundingBox.AddPoint(p.GetDataPointer());
    }
    boundingBox.Inflate(10);
}

BinaryImageType::Pointer meshToBinaryImage(MeshType::Pointer mesh, unsigned imageResolution, double imageMargin) {
typedef itk::TriangleMeshToBinaryImageFilter<MeshType, BinaryImageType> TriangleMeshToBinaryImageFilterType;

    // We transform the mesh to a binary image
    TriangleMeshToBinaryImageFilterType::Pointer meshToBinImageFilter = TriangleMeshToBinaryImageFilterType::New();


    // we choose the image slightly larger than the bounding box of the mesh
    const MeshType::BoundingBoxType* boundingBox = mesh->GetBoundingBox();
    PointType minPt = boundingBox->GetMinimum();
    for (unsigned d = 0; d < 3; d++) {minPt.SetElement(d, minPt.GetElement(d) - imageMargin); }
    PointType maxPt = boundingBox->GetMaximum();
    for (unsigned d = 0; d < 3; d++) {maxPt.SetElement(d, maxPt.GetElement(d) + imageMargin); }

    meshToBinImageFilter->SetOrigin(minPt);
    TriangleMeshToBinaryImageFilterType::SizeType size;
    for (unsigned d = 0; d < 3; d++) {
    size[d] = imageResolution;
    }

    meshToBinImageFilter->SetSize(size);
    TriangleMeshToBinaryImageFilterType::SpacingType spacing;
    for (unsigned d = 0; d < 3; d++) { spacing[d] = (maxPt[d] - minPt[d]) / size[d]; }
   meshToBinImageFilter->SetSpacing(spacing);

    meshToBinImageFilter->SetInput(mesh);
    meshToBinImageFilter->Update();
    return meshToBinImageFilter->GetOutput();
}

mitk::PointSet::PointType
mitk::PointSet::GetPoint( PointIdentifier id, int t ) const
{
    PointType out;
    out.Fill(0);

    if ( (unsigned int) t >= m_PointSetSeries.size() )
    {
        return out;
    }

    if ( m_PointSetSeries[t]->GetPoints()->IndexExists(id) )
    {
        m_PointSetSeries[t]->GetPoint( id, &out );
        this->GetGeometry(t)->IndexToWorld( out, out );
        return out;
    }
    else
    {
        return out;
    }
}

bool testNorms()
{
  typedef PointVector<3, int> PointType;
  PointType aPoint;

  aPoint.at ( 2 ) =  2;
  aPoint.at ( 1 ) = -1;
  aPoint.at ( 0 ) =  3;

  trace.beginBlock ( "Test of Norms" );
  trace.info() << "aPoint l_2 norm="<<aPoint.norm() <<endl;
  trace.info() << "aPoint l_1 norm="<<aPoint.norm ( PointType::L_1 ) <<endl;
  trace.info() << "aPoint l_infty norm="<<aPoint.norm ( PointType::L_infty ) <<endl;

  trace.info() << "Normalization="<<aPoint.getNormalized () <<endl;

  trace.endBlock();


  return ( ( aPoint.norm ( PointType::L_1 ) == 6 ) &&
     ( aPoint.norm ( PointType::L_infty ) == 3 ) );

}

void Optimizer::optimizeUseG2O()
{


    // create the linear solver
    BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();

    // create the block solver on top of the linear solver
    BlockSolverX* blockSolver = new BlockSolverX(linearSolver);

    // create the algorithm to carry out the optimization
    //OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
    OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);

    // NOTE: We skip to fix a variable here, either this is stored in the file
    // itself or Levenberg will handle it.

    // create the optimizer to load the data and carry out the optimization
    SparseOptimizer optimizer;
    SparseOptimizer::initMultiThreading();
    optimizer.setVerbose(true);
    optimizer.setAlgorithm(optimizationAlgorithm);

    {
        pcl::ScopeTime time("G2O setup Graph vertices");
        for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
        {
            VertexSE3 *vertex = new VertexSE3;
            vertex->setId(cloud_count);
            Isometry3D affine = Isometry3D::Identity();
            affine.linear() = m_pointClouds[cloud_count]->sensor_orientation_.toRotationMatrix().cast<Isometry3D::Scalar>();
            affine.translation() = m_pointClouds[cloud_count]->sensor_origin_.block<3, 1>(0, 0).cast<Isometry3D::Scalar>();
            vertex->setEstimate(affine);
            optimizer.addVertex(vertex);
        }
        optimizer.vertex(0)->setFixed(true);
    }

    {
        pcl::ScopeTime time("G2O setup Graph edges");
        double trans_noise = 0.5, rot_noise = 0.5235;
        EdgeSE3::InformationType infomation = EdgeSE3::InformationType::Zero();
        infomation.block<3, 3>(0, 0) << trans_noise * trans_noise, 0, 0,
                                        0, trans_noise * trans_noise, 0,
                                        0, 0, trans_noise * trans_noise;
        infomation.block<3, 3>(3, 3) << rot_noise * rot_noise, 0, 0,
                                        0, rot_noise * rot_noise, 0,
                                        0, 0, rot_noise * rot_noise;
        for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count)
        {
            CloudPair pair = m_cloudPairs[pair_count];
		    int from = pair.corresIdx.first;
		    int to = pair.corresIdx.second;
            EdgeSE3 *edge = new EdgeSE3;
		    edge->vertices()[0] = optimizer.vertex(from);
		    edge->vertices()[1] = optimizer.vertex(to);

            Eigen::Matrix<double, 6, 6> ATA = Eigen::Matrix<double, 6, 6>::Zero();
            Eigen::Matrix<double, 6, 1> ATb = Eigen::Matrix<double, 6, 1>::Zero();
#pragma unroll 8
            for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
                int point_p = pair.corresPointIdx[point_count].first;
                int point_q = pair.corresPointIdx[point_count].second;
                PointType P = m_pointClouds[from]->points[point_p];
                PointType Q = m_pointClouds[to]->points[point_q];

                Eigen::Vector3d p = P.getVector3fMap().cast<double>();
                Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
                Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();

                double b = (p - q).dot(Np);

                Eigen::Matrix<double, 6, 1> A_p;
                A_p.block<3, 1>(0, 0) = p.cross(Np);
                A_p.block<3, 1>(3, 0) = Np;

                ATA += A_p * A_p.transpose();
                ATb += A_p * b;
            }

            Eigen::Matrix<double, 6, 1> X = ATA.ldlt().solve(ATb);
            Isometry3D measure = Isometry3D::Identity();
            float beta = X[0];
            float gammar = X[1];
            float alpha = X[2];
            measure.linear() = (Eigen::Matrix3d)Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(gammar, Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitX());
            measure.translation() = X.block<3, 1>(3, 0);

            edge->setMeasurement(measure);

		    edge->setInformation(infomation);
            
            optimizer.addEdge(edge);
        }
    }

    optimizer.save("debug_preOpt.g2o");
    {
//.........这里部分代码省略.........