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

int main ( int argc, char** argv )
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source ( new pcl::PointCloud<pcl::PointXYZ> () );
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target ( new pcl::PointCloud<pcl::PointXYZ> () );

    loadFile ( argv[1], *cloud_source );


    float *R, *t;
    R = new float [9];
    t = new float [3];
    loadRTfromFile(R, t, argv[3]);
    Eigen::Affine3f RT;
    RT.matrix() <<
                R[0], R[1], R[2], t[0],
                R[3], R[4], R[5], t[1],
                R[6], R[7], R[8], t[2],
                0,0,0,1;
    delete [] R;
    delete [] t;


    pcl::transformPointCloud ( *cloud_source, *cloud_target, RT );

    pcl::io::savePCDFileASCII ( argv[2], *cloud_target );

    return 0;
}

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;
}

void fuzzyAffines()
{
    std::vector<Eigen::Matrix4f> trans;
    trans.reserve(count/10);
    for( size_t i=0; i<count/10; i++ )
    {
        Eigen::Vector3f x = Eigen::Vector3f::Random();
        Eigen::Vector3f y = Eigen::Vector3f::Random();

        x.normalize();
        y.normalize();

        Eigen::Vector3f z = x.cross(y);
        z.normalize();

        y = z.cross(x);
        y.normalize();

        Eigen::Affine3f t = Eigen::Affine3f::Identity();
        Eigen::Matrix3f r = Eigen::Matrix3f::Identity();

        r.col(0) = x;
        r.col(1) = y;
        r.col(2) = z;

        t.rotate(r);
        t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );

        trans.push_back( t.matrix() );
    }

    s_plot.setColor( Eigen::Vector4f(1,0,0,1) );
    s_plot.setLineWidth( 3.0 );
    s_plot( trans, nox::plot<float>::Pos | nox::plot<float>::CS );
}

 /**
  * @brief Return the modelview matrix as a GLdouble array.
  *
  * Similar to OpenGL old method using glGet**(GL_MODELVIEW_MATRIX)
  * @param matrix A pointer to a GLdouble of at least 16 elements to fill with view matrix.
  */
 void getViewMatrix (GLdouble *matrix)
 {
     Eigen::Matrix4f mv = view_matrix.matrix();
     for (int i = 0; i < 16; ++i)
     {
         matrix[i] = mv(i);
     }
 }

gtsam::Pose3 transformToPose3(const Eigen::Affine3f& tform)
{
  //  return gtsam::Pose3(gtsam::Rot3(tform(0,0),tform(0,1),tform(0,2),
  //				  tform(1,0),tform(1,1),tform(1,2),
  //				  tform(2,0),tform(2,1),tform(2,2),
  //				  gtsam::Point3(tform());
  return gtsam::Pose3(tform.matrix().cast<double>());
}

void TrackerICP::determineTransformation(PointCloudConstPtr pointCloud, Eigen::Affine3f &T, bool &converged, float &RMS){

    // Filter
    approximateVoxelFilter->setInputCloud(pointCloud);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr filteredPointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    approximateVoxelFilter->filter(*filteredPointCloud);
    std::cout << "Reduced number of points in source from " << pointCloud->points.size() << " to " << filteredPointCloud->points.size() << std::endl;

////pcl::io::savePLYFileBinary(fileName.toStdString(), *pointCloudPCL);
//pcl::PLYWriter w;
//// Write to ply in binary without camera
//w.write<pcl::PointXYZRGB> ("filteredPointCloud.ply", *filteredPointCloud, true, false);

    // Add normal fields to source (due to PCL bug)
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::copyPointCloud(*filteredPointCloud, *pointCloudNormals);

//    correspondenceEstimator->setInputSource(pointCloudNormals);
//    correspondenceRejectorBoundary->setInputSource<pcl::PointXYZRGBNormal>(pointCloudNormals);
    icp->setInputSource(pointCloudNormals);

    // Align
    pcl::PointCloud<pcl::PointXYZRGBNormal> registeredPointCloud;
    icp->align(registeredPointCloud, lastTransformation.matrix());

    //std::cout << "Nr of iterations: " << icp->nr_iterations_ << std::endl;

    // Computes nearest neighbors from scratch
    //std::cout << "Mean error: " << icp->getFitnessScore(100.0) << std::endl;

    pcl::Correspondences correspondences = *(icp->correspondences_);
    int nCorrespondences = correspondences.size();
    std::cout << "Number of correspondences: " << nCorrespondences << std::endl;
    float sumDistances = 0.0;
    for(int i=0; i<nCorrespondences; i++){
       sumDistances += correspondences[i].distance;
    }
    RMS = sumDistances/nCorrespondences;
    std::cout << "RMS: " << RMS << std::endl;

    //std::cout << "Median distance: " << correspondenceRejectorMedian->getMedianDistance() << std::endl;

    converged = icp->hasConverged();

    std::cout << "Converged: " << converged << std::endl;

    if(converged){
        Eigen::Affine3f Traw;
        Traw.matrix() = icp->getFinalTransformation();
        poseFilter->filterPoseEstimate(Traw, T);
        //T = Traw;
        lastTransformation = T;
    } else {
        T = lastTransformation;
    }

}

keypoint_map::keypoint_map(const std::string & dir_name) {

	init_feature_detector(fd, de, dm);

	intrinsics << 525.0, 525.0, 319.5, 239.5;

	pcl::io::loadPCDFile(dir_name + "/keypoints3d.pcd", keypoints3d);

	cv::FileStorage fs(dir_name + "/descriptors.yml", cv::FileStorage::READ);

	fs["descriptors"] >> descriptors;
	fs["weights"] >> weights;

	cv::FileNode obs = fs["observations"];
	for (cv::FileNodeIterator it = obs.begin(); it != obs.end(); ++it) {
		observation o;
		*it >> o;
		observations.push_back(o);
	}

	cv::FileNode cam_pos = fs["camera_positions"];
	for (cv::FileNodeIterator it = cam_pos.begin(); it != cam_pos.end(); ++it) {
		Eigen::Affine3f pos;

		int i = 0;
		for (cv::FileNodeIterator it2 = (*it).begin(); it2 != (*it).end();
				++it2) {
			int u = i / 4;
			int v = i % 4;

			*it2 >> pos.matrix().coeffRef(u, v);
			i++;

		}
		camera_positions.push_back(pos);
	}

	fs.release();

	for (size_t i = 0; i < camera_positions.size(); i++) {
		cv::Mat rgb = cv::imread(
				dir_name + "/rgb/" + boost::lexical_cast<std::string>(i)
						+ ".png", CV_LOAD_IMAGE_UNCHANGED);
		cv::Mat depth = cv::imread(
				dir_name + "/depth/" + boost::lexical_cast<std::string>(i)
						+ ".png", CV_LOAD_IMAGE_UNCHANGED);

		rgb_imgs.push_back(rgb);
		depth_imgs.push_back(depth);
	}

}

void alignedAffines()
{
    s_plot.setColor( Eigen::Vector4f(0,0,0,1) );
    s_plot.setLineWidth( 1.0 );
    for( size_t i=0; i<count; i++ )
    {
        Eigen::Affine3f t = Eigen::Affine3f::Identity();
        t.rotate(Eigen::Matrix3f::Identity());
        t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );

        s_plot( t.matrix(), nox::plot<float>::Pos | nox::plot<float>::CS );
    }
}

void alignScaleOnce(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2)
{
  Eigen::Vector4f center1, center2;
  Eigen::Matrix3f covariance1, covariance2;
  pcl::computeMeanAndCovarianceMatrix (*cloud1, covariance1, center1);
  pcl::computeMeanAndCovarianceMatrix (*cloud2, covariance2, center2);

  std::cout << "scale1 = " << covariance1.trace() << std::endl;
  std::cout << "scale2 = " << covariance2.trace() << std::endl;
  
  
  Eigen::Affine3f Scale;
  float s;
  s = 1.0 / sqrt( covariance1.trace() );
  s *= sqrt(0.003); // ad-hoc for numerical issue
  Scale.matrix() <<
  s, 0, 0, 0,
  0, s, 0, 0,
  0, 0, s, 0,
  0, 0, 0, 1;
  pcl::transformPointCloud ( *cloud1, *cloud1, Scale );

  s = 1.0 / sqrt( covariance2.trace() );
  s *= sqrt(0.003); // ad-hoc for numerical issue
  Scale.matrix() <<
  s, 0, 0, 0,
  0, s, 0, 0,
  0, 0, s, 0,
  0, 0, 0, 1;
  pcl::transformPointCloud ( *cloud2, *cloud2, Scale );

  pcl::computeMeanAndCovarianceMatrix (*cloud1, covariance1, center1);
  pcl::computeMeanAndCovarianceMatrix (*cloud2, covariance2, center2);

  std::cout << "scale1 = " << covariance1.trace() << std::endl;
  std::cout << "scale2 = " << covariance2.trace() << std::endl;
}

bool update(glh::App* app)
{
    float t = (float) app->time();
    angle += (t - tprev) * radial_speed;
    tprev = t;
    Eigen::Affine3f transform;
    transform = Eigen::AngleAxis<float>(angle, glh::vec3(0.f, 0.f, 1.f));
    obj2world = transform.matrix();

    env.set_mat4(OBJ2WORLD, obj2world);
    env.set_texture2d("Sampler", texture);

    return g_run;
}

/**
 * ./collar-lines-odom $(ls *.bin | sort | xargs)
 */
int main(int argc, char** argv) {

    vector<string> clouds_to_process;
    string output_filename;
    if(!parse_arguments(argc, argv, output_filename, clouds_to_process)) {
      return EXIT_FAILURE;
    }
    ofstream output_file(output_filename.c_str());

    LoamImuInput imu(SCAN_PERIOD);
    ScanRegistration scanReg(imu, SCAN_PERIOD);
    LaserOdometry laserOdom(SCAN_PERIOD);
    LaserMapping mapping(SCAN_PERIOD);


    VelodynePointCloud cloud;
    float time = 0;
    for (int i = 0; i < clouds_to_process.size(); i++) {
        string filename = clouds_to_process[i];
        cerr << "KITTI file: " << filename << endl << flush;

        if (endsWith(filename, ".bin")) {
          but_velodyne::VelodynePointCloud::fromKitti(filename, cloud);
          pcl::transformPointCloud(cloud, cloud,
              cloud.getAxisCorrection().inverse().matrix());
        } else {
          pcl::io::loadPCDFile(filename, cloud);
        }

        LaserOdometry::Inputs odomInputs;
        scanReg.run(cloud, time, odomInputs);

        LaserMapping::Inputs mappingInputs;
        laserOdom.run(odomInputs, mappingInputs);

        LaserMapping::Outputs mappingOutputs;
        mapping.run(mappingInputs, mappingOutputs, time);

        Eigen::Affine3f transf = pcl::getTransformation(-mappingOutputs.transformToMap[3],
            -mappingOutputs.transformToMap[4], mappingOutputs.transformToMap[5], -mappingOutputs.transformToMap[0],
            -mappingOutputs.transformToMap[1], mappingOutputs.transformToMap[2]);
        but_velodyne::KittiUtils::printPose(output_file, transf.matrix());

        time += SCAN_PERIOD;
    }

    output_file.close();

    return EXIT_SUCCESS;
}

int main( )
  {
  Eigen::VectorXf a(3);
  Eigen::VectorXf b(a);
  Eigen::VectorXf c(b);

  a = Eigen::Vector4f(1, 2, 3, 4);
  b = Eigen::Vector4f(3, 2, 1, 0);

  c = a + b;

  Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> x(1,3);
  Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> y(3,1);

  x = y;

  x.resize(4,4);
  x.resize(2,3);

  y = x;

  y = a;

  Eigen::Vector3f d = a.block<3, 1>(0, 0);

  c = a;

  Eigen::Vector3f e = a.block<3, 1>(0, 0);

  Eigen::ArrayXf ddd = c;

  ddd = a;

  Eigen::Matrix3f f;

  f.row(0) = e;

  Eigen::Affine3f m;


  m.matrix().row(0).head<3>() = e;

  return EXIT_SUCCESS;
  }

void SLPointCloudWidget::updateCalibration(){

    CalibrationData calibration;
    bool load_result = calibration.load("calibration.xml");
	if (!load_result)
		return;
    // Camera coordinate system
    visualizer->addCoordinateSystem(50, "camera", 0);

    // Projector coordinate system
    cv::Mat TransformPCV(4, 4, CV_32F, 0.0);
    cv::Mat(calibration.Rp).copyTo(TransformPCV.colRange(0, 3).rowRange(0, 3));
    cv::Mat(calibration.Tp).copyTo(TransformPCV.col(3).rowRange(0, 3));
    TransformPCV.at<float>(3, 3) = 1.0; // make it homogeneous 
    Eigen::Affine3f TransformP;
    cv::cv2eigen(TransformPCV, TransformP.matrix());

    visualizer->addCoordinateSystem(50, TransformP.inverse(), "projector", 0);
}

void alignScaleOnce1(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2)
{
  Eigen::Vector4f center1, center2;
  Eigen::Matrix3f covariance1, covariance2;
  pcl::computeMeanAndCovarianceMatrix (*cloud1, covariance1, center1);
  pcl::computeMeanAndCovarianceMatrix (*cloud2, covariance2, center2);

  // symmetric scale estimation by Horn 1968
  float s = sqrt( covariance1.trace() / covariance2.trace() );
  
  Eigen::Affine3f Scale;
  Scale.matrix() <<
  s, 0, 0, 0,
  0, s, 0, 0,
  0, 0, s, 0,
  0, 0, 0, 1;
  
  pcl::transformPointCloud ( *cloud2, *cloud2, Scale );

}

void addVelodynePcl(Visualizer3D &vis, const VelodynePointCloud &cloud, const Eigen::Affine3f &pose) {
  PointCloud<PointXYZRGB>::Ptr rgb_cloud(new PointCloud<PointXYZRGB>());

  float min = cloud.getMinValuePt().intensity;
  float max = cloud.getMaxValuePt().intensity;

  for(VelodynePointCloud::const_iterator pt = cloud.begin(); pt < cloud.end(); pt++) {
    uchar r, g, b;
    float normalized = (pt->intensity - min) / (max - min) * 255.0;
    toColor(MIN(normalized*2, 255), r, g, b);
    PointXYZRGB rgb_pt;
    rgb_pt.x = pt->x;
    rgb_pt.y = pt->y;
    rgb_pt.z = pt->z;
    rgb_pt.r = r;
    rgb_pt.g = g;
    rgb_pt.b = b;
    rgb_cloud->push_back(rgb_pt);
  }

  vis.addColorPointCloud(rgb_cloud, pose.matrix());
}