C++ Affine3f::setIdentity方法代码示例

void ndtTransformAndAdd(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& A,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& B)
{
	 pcl::NormalDistributionsTransform<pcl::PointXYZRGB, pcl::PointXYZRGB> ndt;
	 // Setting scale dependent NDT parameters
	 // Setting minimum transformation difference for termination condition.
	 ndt.setTransformationEpsilon (0.01);
	 // Setting maximum step size for More-Thuente line search.
	 ndt.setStepSize (0.1);
	 //Setting Resolution of NDT grid structure (VoxelGridCovariance).
	 ndt.setResolution (1.0);

	  // Setting max number of registration iterations.
	  ndt.setMaximumIterations (35);

	  // Setting point cloud to be aligned.
	 ndt.setInputSource (B);
	  // Setting point cloud to be aligned to.
	 ndt.setInputTarget (A);

	 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	 Eigen::Affine3f estimate;
	 estimate.setIdentity();
	 ndt.align (*output_cloud, estimate.matrix());

	 std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
	           << " score: " << ndt.getFitnessScore () << std::endl;

	   // Transforming unfiltered, input cloud using found transform.
	 pcl::transformPointCloud (*B, *output_cloud, ndt.getFinalTransformation ());

	 *A=*A+*output_cloud;
}

bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
                     Eigen::Affine3f& transf)
{
  // uz: versor pointing toward the destination
  Eigen::Vector3f uz = target - rayo;
  if (std::abs(uz.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
    return false;
  }
  uz.normalize();
  //std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
  // ux: versor pointing toward the ground
  Eigen::Vector3f ux = down - down.dot(uz) * uz;  
  if (std::abs(ux.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
    return false;
  }
  ux.normalize();
  //std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
  Eigen::Vector3f uy = uz.cross(ux);
  //std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
  Eigen::Matrix3f rot;
  rot << ux.x(), uy.x(), uz.x(),
         ux.y(), uy.y(), uz.y(),
         ux.z(), uy.z(), uz.z();
  transf.setIdentity();
  transf.translate(rayo);
  transf.rotate(rot);
  //std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
  return true;
}

void CData4Viewer::drawCameraView(qglviewer::Camera* pCamera_)
{
	_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f);
	
	glMatrixMode(GL_MODELVIEW);
	Eigen::Affine3f prj_w_t_c; _pTracker->getCurrentProjectionMatrix(&prj_w_t_c);
	Eigen::Affine3f init; init.setIdentity(); init(1, 1) = -1.f; init(2, 2) = -1.f;// rotate the default opengl camera orientation to make it facing positive z
	glLoadMatrixf(init.data());
	glMultMatrixf(prj_w_t_c.data());

	//if(_bShowCamera) {
	//	_pTracker->displayGlobalRelocalization();
	//}
	//if(_bShowMarkers) {
	//	_pTracker->displayAllGlobalFeatures(_nVoxelLevel,_bRenderSphere);
	//}

	_pVirtualCameraView->assignRTfromGL();
	_pCubicGrids->rayCast(&*_pVirtualCameraView,true,_bCapture); //get virtual frame
	bool bLightingStatus = _pGL->_bEnableLighting;
	_pGL->_bEnableLighting = true;
	_pVirtualCameraView->gpuRender3DPts(_pGL.get(),_pGL->_usLevel);
	_pGL->_bEnableLighting = bLightingStatus;
	//PRINTSTR("drawCameraView");
	return;	
}

PointCloudViewer::PointCloudViewer(QWidget* parent, Qt::WindowFlags f): QVTKWidget(parent, f)
{
    mImpl = new PointCloudViewer::Impl;
    mImpl->Vis.addPointCloud(common::KinectPointCloud::Ptr(new common::KinectPointCloud));
    Eigen::Affine3f trans;
    trans.setIdentity();
    trans.rotate(Eigen::AngleAxisf(3.14159265, Eigen::Vector3f(0, 0, 1)));
    
    mImpl->Vis.addCoordinateSystem(1.0, trans);
    mImpl->Vis.setBackgroundColor(0, 0, 0);
    SetRenderWindow(mImpl->Vis.getRenderWindow().GetPointer());
}

void CData4Viewer::drawRGBView()
{
	_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f);

	glMatrixMode ( GL_MODELVIEW );
	Eigen::Affine3f tmp; tmp.setIdentity();
	Matrix4f mv = btl::utility::setModelViewGLfromPrj(tmp); //mv transform X_m to X_w i.e. model coordinate to world coordinate
	glLoadMatrixf( mv.data() );
	_pKinect->_pRGBCamera->renderCameraInLocal(*_pKinect->_pCurrFrame->_acvgmShrPtrPyrRGBs[_pGL->_usLevel],  _pGL.get(),false, NULL, 0.2f, true ); //render in model coordinate
	//PRINTSTR("drawRGBView");
    return;
}

int
 main (int argc, char** argv)
{
    // Get input object and scene
    if (argc < 2)
    {
        pcl::console::print_error ("Syntax is: %s cloud1.pcd (cloud2.pcd)\n", argv[0]);
        return (1);
    }
    
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);

    // Load object and scene
    pcl::console::print_highlight ("Loading point clouds...\n");
    if(argc<3)
    {
        if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0)
            pcl::console::print_error ("Error loading first file!\n");
        *cloud_out = *cloud_in;
        
        //transform cloud
        Eigen::Affine3f transformation;
        transformation.setIdentity();
        transformation.translate(Eigen::Vector3f(0.3,0.02,-0.1));
        float roll, pitch, yaw;
        roll = 0.02; pitch = 1.2; yaw = 0;
        Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
        Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
        Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
        Eigen::Quaternion<float> q = rollAngle*pitchAngle*yawAngle;
        transformation.rotate(q);
        
        pcl::transformPointCloud<pcl::PointXYZRGBA>(*cloud_in, *cloud_out, transformation);
        std::cout << "Transformed " << cloud_in->points.size () << " data points:"
            << std::endl;
    }else{
       if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0 ||
        pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_out) < 0)
        {
            pcl::console::print_error ("Error loading files!\n");
            return (1);
        } 
    }
    
    // Fill in the CloudIn data
//     cloud_in->width    = 100;
//     cloud_in->height   = 1;
//     cloud_in->is_dense = false;
//     cloud_in->points.resize (cloud_in->width * cloud_in->height);
//     for (size_t i = 0; i < cloud_in->points.size (); ++i)
//     {
//         cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
//         cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
//         cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
//     }

    std::cout << "size:" << cloud_out->points.size() << std::endl;
      
    {
        pcl::ScopeTime("icp proces");
        
        pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
        icp.setInputSource(cloud_in);
        icp.setInputTarget(cloud_out);
        pcl::PointCloud<pcl::PointXYZRGBA> Final;
        icp.setMaximumIterations(1000000);
        icp.setRANSACOutlierRejectionThreshold(0.01);
        icp.align(Final);
        std::cout << "has converged:" << icp.hasConverged() << " score: " <<
        icp.getFitnessScore() << std::endl;
        std::cout << icp.getFinalTransformation() << std::endl;
        
        //translation, rotation
        Eigen::Matrix4f icp_transformation=icp.getFinalTransformation();
        Eigen::Matrix3f icp_rotation = icp_transformation.block<3,3>(0,0);
        Eigen::Vector3f euler = icp_rotation.eulerAngles(0,1,2);
        std::cout << "rotation: " << euler.transpose() << std::endl;
        std::cout << "translation:" << icp_transformation.block<3,1>(0,3).transpose() << std::endl;
    }
  

 return (0);
}

void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
                                                    const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
  JSK_ROS_DEBUG("DepthImageCreator::publish_points");
  if (!pcloud2)  return;
  bool proc_cloud = true, proc_image = true, proc_disp = true;
  if ( pub_cloud_.getNumSubscribers()==0 ) {
    proc_cloud = false;
  }
  if ( pub_image_.getNumSubscribers()==0 ) {
    proc_image = false;
  }
  if ( pub_disp_image_.getNumSubscribers()==0 ) {
    proc_disp = false;
  }
  if( !proc_cloud && !proc_image && !proc_disp) return;

  int width = info->width;
  int height = info->height;
  float fx = info->P[0];
  float cx = info->P[2];
  float tx = info->P[3];
  float fy = info->P[5];
  float cy = info->P[6];

  Eigen::Affine3f sensorPose;
  {
    tf::StampedTransform transform;
    if(use_fixed_transform) {
      transform = fixed_transform;
    } else {
      try {
        tf_listener_->waitForTransform(pcloud2->header.frame_id,
                                       info->header.frame_id,
                                       info->header.stamp,
                                       ros::Duration(0.001));
        tf_listener_->lookupTransform(pcloud2->header.frame_id,
                                      info->header.frame_id,
                                      info->header.stamp, transform);
      }
      catch ( std::runtime_error e ) {
        JSK_ROS_ERROR("%s",e.what());
        return;
      }
    }
    tf::Vector3 p = transform.getOrigin();
    tf::Quaternion q = transform.getRotation();
    sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
    Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
    sensorPose = sensorPose * rot;

    if (tx != 0.0) {
      Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
      sensorPose = sensorPose * trans;
    }
#if 0 // debug print
    JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
             sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
             sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
             sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
             sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
#endif
  }

  PointCloud pointCloud;
  pcl::RangeImagePlanar rangeImageP;
  {
    // code here is dirty, some bag is in RangeImagePlanar
    PointCloud tpc;
    pcl::fromROSMsg(*pcloud2, tpc);

    Eigen::Affine3f inv;
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
    inv = sensorPose.inverse();
    pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
#else
    pcl::getInverse(sensorPose, inv);
    pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
#endif

    Eigen::Affine3f dummytrans;
    dummytrans.setIdentity();
    rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
                                                   width/scale_depth, height/scale_depth,
                                                   cx/scale_depth, cy/scale_depth,
                                                   fx/scale_depth, fy/scale_depth,
                                                   dummytrans); //sensorPose);
  }

  cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
  float *tmpf = (float *)mat.ptr();
  for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
    tmpf[i] = rangeImageP.points[i].z;
  }

  if(scale_depth != 1.0) {
    cv::Mat tmpmat(info->height, info->width, CV_32FC1);
    cv::resize(mat, tmpmat, cv::Size(info->width, info->height)); // LINEAR
    //cv::resize(mat, tmpmat, cv::Size(info->width, info->height), 0.0, 0.0, cv::INTER_NEAREST);
    mat = tmpmat;
  }
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                "Error while parsing \"%s\": unexpected content at %s",
                filename, offset(node.offset_debug()));

        /* Look up the name of the current element */
        auto it = tags.find(node.name());
        if (it == tags.end())
            throw NoriException("Error while parsing \"%s\": unexpected tag \"%s\" at %s",
            filename, node.name(), offset(node.offset_debug()));
        int tag = it->second;

        /* Perform some safety checks to make sure that the XML tree really makes sense */
        bool hasParent            = parentTag != EInvalid;
        bool parentIsObject       = hasParent && parentTag < NoriObject::EClassTypeCount;
        bool currentIsObject      = tag < NoriObject::EClassTypeCount;
        bool parentIsTransform    = parentTag == ETransform;
        bool currentIsTransformOp = tag == ETranslate || tag == ERotate || tag == EScale || tag == ELookAt || tag == EMatrix;

        if (!hasParent && !currentIsObject)
            throw NoriException("Error while parsing \"%s\": root element \"%s\" must be a Nori object (at %s)",
            filename, node.name(), offset(node.offset_debug()));

        if (parentIsTransform != currentIsTransformOp)
            throw NoriException("Error while parsing \"%s\": transform nodes "
            "can only contain transform operations (at %s)",
            filename,  offset(node.offset_debug()));

        if (hasParent && !parentIsObject && !(parentIsTransform && currentIsTransformOp))
            throw NoriException("Error while parsing \"%s\": node \"%s\" requires a Nori object as parent (at %s)",
            filename, node.name(), offset(node.offset_debug()));

        if (tag == EScene)
            node.append_attribute("type") = "scene";
        else if (tag == ETransform)
            transform.setIdentity();

        PropertyList propList;
        std::vector<NoriObject *> children;
        for (pugi::xml_node &ch: node.children()) {
            NoriObject *child = parseTag(ch, propList, tag);
            if (child)
                children.push_back(child);
        }

        NoriObject *result = nullptr;
        try {
            if (currentIsObject) {
                check_attributes(node, { "type" });

                /* This is an object, first instantiate it */
                result = NoriObjectFactory::createInstance(
                             node.attribute("type").value(),
                             propList
                         );

                if (result->getClassType() != (int) tag) {
                    throw NoriException(
                        "Unexpectedly constructed an object "
                        "of type <%s> (expected type <%s>): %s",
                        NoriObject::classTypeName(result->getClassType()),
                        NoriObject::classTypeName((NoriObject::EClassType) tag),
                        result->toString());
                }

                /* Add all children */
                for (auto ch: children) {
                    result->addChild(ch);

Eigen::Affine3f OpenNiInterfacePlain::getCloudPose() {
    Eigen::Affine3f mid;
    mid.setIdentity();
    return mid;
}