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

void GraphicEnd::savepcd()
{
    cout<<"save final pointcloud"<<endl;
    SLAMEnd slam;
    slam.init(NULL);
    SparseOptimizer& opt = slam.globalOptimizer;
    opt.load("/home/lc/workspace/paper_related/Appolo/test/result/final_after.g2o");
    ifstream fin("/home/lc/workspace/paper_related/Appolo/test/result/key_frame.txt");
    PointCloud::Ptr output(new PointCloud());
    PointCloud::Ptr curr( new PointCloud());
    pcl::VoxelGrid<PointT> voxel;
    voxel.setLeafSize(0.01f, 0.01f, 0.01f );
    string pclPath ="/media/新加卷/dataset/dataset1/pcd/";

    pcl::PassThrough<PointT> pass;
    pass.setFilterFieldName("z");
    double z = 5.0;
    pass.setFilterLimits(0.0, z);
    
    while( !fin.eof() )
    {
        int frame, id;
        fin>>id>>frame;
        ss<<pclPath<<frame<<".pcd";
        
        string str;
        ss>>str;
        cout<<"loading "<<str<<endl;
        ss.clear();

        pcl::io::loadPCDFile( str.c_str(), *curr );
        //cout<<"curr cloud size is: "<<curr->points.size()<<endl;
        VertexSE3* pv = dynamic_cast<VertexSE3*> (opt.vertex( id ));
        if (pv == NULL)
            break;
        Eigen::Isometry3d pos = pv->estimate();

        cout<<pos.matrix()<<endl;
        voxel.setInputCloud( curr );
        PointCloud::Ptr tmp( new PointCloud());
        voxel.filter( *tmp );
        curr.swap( tmp );
        pass.setInputCloud( curr );
        pass.filter(*tmp);
        curr->swap( *tmp );
        //cout<<"tmp: "<<tmp->points.size()<<endl;
        pcl::transformPointCloud( *curr, *tmp, pos.matrix());
        *output += *tmp;
        //cout<<"output: "<<output->points.size()<<endl;
    }
    voxel.setInputCloud( output );
    PointCloud::Ptr output_filtered( new PointCloud );
    voxel.filter( *output_filtered );
    output->swap( *output_filtered );
    //cout<<output->points.size()<<endl;
    pcl::io::savePCDFile( "/home/lc/workspace/paper_related/Appolo/test/result/result.pcd", *output);
    cout<<"final result saved."<<endl;
}

void GraphicEnd::generateKeyFrame( Eigen::Isometry3d T )
{
    cout<<BOLDGREEN<<"GraphicEnd::generateKeyFrame"<<RESET<<endl;
    //把present中的数据存储到current中
    _currKF.id ++;
    _currKF.planes = _present.planes;
    _currKF.frame_index = _index;
    _lastRGB = _currRGB.clone();
    _kf_pos = _robot;

    cout<<"add key frame: "<<_currKF.id<<endl;
    //waitKey(0);
    _keyframes.push_back( _currKF );
    
    //将current关键帧存储至全局优化器
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //顶点
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    //v->setEstimate( _robot );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    //v->setEstimate( Eigen::Isometry3d::Identity() );
    else
        v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );
    //边
    EdgeSE3* edge = new EdgeSE3();
    edge->vertices()[0] = opt.vertex( _currKF.id - 1 );
    edge->vertices()[1] = opt.vertex( _currKF.id );
    Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
    information(0, 0) = information(2,2) = 100;
    information(1, 1) = 100;
    information(3,3) = information(4,4) = information(5,5) = 100; 
    edge->setInformation( information );
    edge->setMeasurement( T );
    opt.addEdge( edge );

    if (_use_odometry)
    {
        Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
        cout<<"odo last = "<<endl<<_odo_last.matrix()<<endl<<"odo this = "<<endl<<_odo_this.matrix()<<endl;
        cout<<"To = "<<endl<<To.matrix()<<endl;
        cout<<"Measurement = "<<endl<<T.matrix()<<endl;
        //waitKey( 0 );
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices()[0] = opt.vertex( _currKF.id - 1 );
        edge->vertices()[1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(1,1) = information(2,2) = information(3,3) = information(4,4) = information(5,5) = 1/(_error_odometry*_error_odometry);
        edge->setInformation( information );
        edge->setMeasurement( To );
        opt.addEdge( edge );
        _odo_last = _odo_this;
    }
    
}

int main()
{
    Mat rgb1,rgb2,depth1,depth2;
    fileread(rgb1,rgb2,depth1,depth2);

    CAMERA_INTRINSIC_PARAMETERS C;
    C.cx = 325.5;
    C.cy = 253.5;
    C.fx = 518.0;
    C.fy = 519.0;
    C.scale = 1000.0;

    //feature detector and descriptor compute
    Eigen::Isometry3d T = transformEstimation(rgb1,rgb2,depth1,depth2,C);

    PointCloud::Ptr cloud1 = image2PointCloud(rgb1,depth1,C);
    PointCloud::Ptr cloud2 = image2PointCloud(rgb2,depth2,C);

    //pcl::io::savePCDFile("1.pcd", *cloud1);
    //pcl::io::savePCDFile("2.pcd", *cloud2);

    cout<<"combining clouds"<<endl;
    PointCloud::Ptr output (new PointCloud());
    pcl::transformPointCloud( *cloud2, *output, T.matrix());
    *cloud1 += *output;
    pcl::io::savePCDFile("result.pcd", *cloud1);
    cout<<"Final result saved."<<endl;
    return 0;
}

/**
 * @function fit_BB
 * @brief 
 */
void fit_BB( std::vector<pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > _clusters ) {
    
    double dim[3]; Eigen::Isometry3d Tf;

    for( int i = 0; i < _clusters.size(); ++i ) {
	printf("\t * Fitting box for cluster %d \n", i );

	pcl::PointCloud<pcl::PointXYZ>::Ptr p( new pcl::PointCloud<pcl::PointXYZ>() );
	p->points.resize( _clusters[i]->points.size() );
	for(int j = 0; j < _clusters[i]->points.size(); ++j ) {
	    p->points[j].x = _clusters[i]->points[j].x;
	    p->points[j].y = _clusters[i]->points[j].y;
	    p->points[j].z = _clusters[i]->points[j].z;
	}
	p->width = 1; p->height = p->points.size();

	// Get Bounding Box
	pointcloudToBB( p, dim, Tf );
	// Store (cube)
	pcl::PointCloud<pcl::PointXYZ>::Ptr pts( new pcl::PointCloud<pcl::PointXYZ>() );
	pcl::PointCloud<pcl::PointXYZ>::Ptr final( new pcl::PointCloud<pcl::PointXYZ>() );

	pts = sampleSQ_uniform( dim[0]/2, dim[1]/2, dim[2]/2, 0.1, 0.1 );
	pcl::transformPointCloud( *pts, *final, Tf.matrix() );

	char name[50];
	sprintf( name, "bb_%d.pcd", i);
	pcl::io::savePCDFileASCII(name, *final );
    }


}

//==============================================================================
dart::dynamics::SkeletonPtr loadSkeletonFromURDF(
    const dart::common::ResourceRetrieverPtr& retriever,
    const dart::common::Uri& uri,
    const Eigen::Isometry3d& transform)
{
  dart::utils::DartLoader urdfLoader;
  const dart::dynamics::SkeletonPtr skeleton
      = urdfLoader.parseSkeleton(uri, retriever);

  if (!skeleton)
    throw std::runtime_error("Unable to load '" + uri.toString() + "'");

  if (skeleton->getNumJoints() == 0)
    throw std::runtime_error("Skeleton is empty.");

  if (!transform.matrix().isIdentity())
  {
    auto freeJoint
        = dynamic_cast<dart::dynamics::FreeJoint*>(skeleton->getRootJoint());

    if (!freeJoint)
      throw std::runtime_error(
          "Unable to cast Skeleton's root joint to FreeJoint.");

    freeJoint->setTransform(transform);
  }

  return skeleton;
}

void
pcl::simulation::RangeLikelihood::applyCameraTransform (const Eigen::Isometry3d & pose)
{
  float T[16];
  Eigen::Matrix4f m = (pose.matrix ().inverse ()).cast<float> ();
  T[0] = m(0,0); T[4] = m(0,1); T[8] = m(0,2); T[12] = m(0,3);
  T[1] = m(1,0); T[5] = m(1,1); T[9] = m(1,2); T[13] = m(1,3);
  T[2] = m(2,0); T[6] = m(2,1); T[10] = m(2,2); T[14] = m(2,3);
  T[3] = m(3,0); T[7] = m(3,1); T[11] = m(3,2); T[15] = m(3,3);
  glMultMatrixf(T);
}

	void TestFKIKFK()
	{
		int arm = RIGHT;
		// Create random legitimate joint values
		std::vector<RobotKin::Joint*> joints = kinematics->linkage("RightArm").joints();
		for (int i = 0; i < joints.size(); i++)
		{
			double jointVal = randbetween(joints[i]->min(), joints[i]->max());
			kinematics->joint(joints[i]->name()).value(jointVal);
		}

		// Do FK to get ee pose
		Eigen::Isometry3d initialFrame;
		initialFrame = kinematics->linkage("RightArm").tool().respectToRobot();

		// Do IK to get joints
		DrcHuboKin::ArmVector q = DrcHuboKin::ArmVector::Zero();
		RobotKin::rk_result_t result = kinematics->armIK(arm, q, initialFrame);

		std::cerr << "Solver result: " << result << std::endl;
		CPPUNIT_ASSERT(RobotKin::RK_SOLVED == result);

		// Do FK and verify that it's pretty much the same
		int baseJoint = 0;
		if (LEFT == arm)
		{ baseJoint = LSP; }
		else if (RIGHT == arm)
		{ baseJoint = RSP; }

		for (int i = baseJoint; i < baseJoint+7; i++)
		{
			//kinematics->joint(DRCHUBO_URDF_JOINT_NAMES[i]).value(q[DRCHUBO_JOINT_INDEX_TO_LIMB_POSITION[i]]);
		}

		Eigen::Isometry3d finalFrame;
		finalFrame = kinematics->linkage("RightArm").tool().respectToRobot();

		//CPPUNIT_ASSERT((initialFrame.matrix() - finalFrame.matrix()).norm() < 0.001);
		std::cerr << "Inital pose: \n" << initialFrame.matrix() << std::endl;
		std::cerr << "Final pose: \n" << finalFrame.matrix() << std::endl;
	}

Eigen::Isometry3d transformEstimation(const Mat& rgb1,const Mat& rgb2,const Mat& depth1,const Mat& depth2,const CAMERA_INTRINSIC_PARAMETERS& C)
{
    vector<KeyPoint> keyPts1,keyPts2;
    Mat descriptors1,descriptors2;

    extractKeypointsAndDescripotrs(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2);


    vector<DMatch> matches;
    descriptorsMatch(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2,matches);


    vector<Point2f> points;
    vector<Point3f> objectPoints;
    vector<Eigen::Vector2d> imagePoints1,imagePoints2;

    getObjectPointsAndImagePoints(depth1,depth2,keyPts1,keyPts2,matches,points,objectPoints,imagePoints1,imagePoints2,C);

    Mat translation,rotation;
    double camera_matrix_data[3][3] = {
        {C.fx, 0, C.cx},
        {0, C.fy, C.cy},
        {0, 0, 1}    };

    Mat cameraMatrix(3,3,CV_64F,camera_matrix_data);


    solvePnPRansac(objectPoints,points,cameraMatrix,Mat(),rotation,translation,false, 100, 1.0, 20);


    Mat rot;
    Rodrigues(rotation,rot);

    Eigen::Matrix3d r;
    Eigen::Vector3d t;

    cout<<rot<<endl;
    cout<<translation<<endl;

    r<< ((double*)rot.data)[0],((double*)rot.data)[1],((double*)rot.data)[2],
            ((double*)rot.data)[3],((double*)rot.data)[4],((double*)rot.data)[5],
            ((double*)rot.data)[6],((double*)rot.data)[7],((double*)rot.data)[8];
    t<<((double*)translation.data)[0],((double*)translation.data)[1],((double*)translation.data)[2];

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(r);
    T.pretranslate(t);
    cout<<T.matrix()<<endl;

    BundleAdjustmentOptimization(objectPoints,imagePoints1,imagePoints2);

    return T;
}

int GraphicEnd2::run()
{
    cout<<"********************"<<endl;
    _present.planes.clear();

    readimage();
    _present.planes.push_back( extractKPandDesp( _currRGB, _currDep ) );
	_present.frame_index = _index;

    RESULT_OF_MULTIPNP result = multiPnP( _currKF.planes, _present.planes, _currKF.frame_index, _present.frame_index );
    Eigen::Isometry3d T = result.T.inverse();

    if (T.matrix() == Eigen::Isometry3d::Identity().matrix())
    {
        //匹配失败
        cout<<BOLDRED"This frame is lost"<<RESET<<endl;
        _lost++;
    }
    else if (result.norm > _max_pos_change)
    {
        //生成新关键帧
        _robot = T*_kf_pos;
        generateKeyFrame(T);
        if (_loop_closure_detection == true)
            loopClosure();
        _lost = 0;
    }
    else
    {
        //位置变化小
        _robot = T*_kf_pos;
        _lost = 0;
    }

    if (_lost > _lost_frames)
    {
        cerr<<"the robot is lost, perform lost recovery"<<endl;
        lostRecovery();
    }

    cout<<RED"keyframe size = "<<_keyframes.size()<<RESET<<endl;
    _index++;
    
    if (_use_odometry )
    {
        _odo_this = _odometry[_index - 1];
    }
        
    return  0;
}

GTEST_TEST(TestLcmUtil, testPose) {
  default_random_engine generator;
  generator.seed(0);
  Eigen::Isometry3d pose;
  pose.linear() = drake::math::UniformlyRandomRotmat(generator);
  pose.translation().setLinSpaced(0, drake::kSpaceDimension);
  pose.makeAffine();
  const Eigen::Isometry3d& const_pose = pose;
  bot_core::position_3d_t msg;
  EncodePose(const_pose, msg);
  Eigen::Isometry3d pose_back = DecodePose(msg);
  EXPECT_TRUE(CompareMatrices(pose.matrix(), pose_back.matrix(), 1e-12,
                              MatrixCompareType::absolute));
}

cv::Point2f FeatureMatcher::point3Dto2D( 
          cv::Point3f& point_xyz, 
    const Eigen::Isometry3d & trans    )
{
  cv::Point2f p; // 3D 点
  Eigen::Vector4d p_xyz1( point_xyz.x, point_xyz.y, point_xyz.z, 1 );
  //变换到参考坐标系下
  Eigen::Matrix<double, 3, 4> K = pCamera->toProjectionMatrix();
  Eigen::Vector3d p_uvw =  K * trans.matrix() * p_xyz1;

  p.x = p_uvw(0) / p_uvw(2);
  p.y = p_uvw(1) / p_uvw(2);
  return p;
}

//==============================================================================
void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform)
{
  if(transform.matrix() == FixedFrame::mAspectProperties.mRelativeTf.matrix())
    return;

  const Eigen::Isometry3d oldTransform = getRelativeTransform();

  FixedFrame::setRelativeTransform(transform);
  dirtyJacobian();
  dirtyJacobianDeriv();

  mRelativeTransformUpdatedSignal.raise(
        this, oldTransform, getRelativeTransform());
}

void FeatureTransformationEstimator::estimatePoseSVD(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d &T)
{
    pcl::TransformationFromCorrespondences tfc;
    for (int i = 0; i < P.cols(); i++) {
        Eigen::Vector3f p_i = P.col(i).cast<float>();
        Eigen::Vector3f q_i = Q.col(i).cast<float>();
        float inverse_weight = p_i(2)*p_i(2) + q_i(2)*q_i(2);
        float weight = 1;
        if (inverse_weight > 0) {
            weight = 1. / weight;
        }
        tfc.add(p_i, q_i, weight);
    }
    T.matrix() = tfc.getTransformation().matrix().cast<double>();
//    T.matrix() = Eigen::umeyama(P, Q, false);
}

int main()
{
	Fastrak fastrak;
	Eigen::Isometry3d pose;
	while (true)
	{
		fastrak.achUpdate();
		for (int i = 0; i < fastrak.getNumChannels(); i++)
		{
			fastrak.getPose(pose, i, false);
			std::cout << "Sensor " << i << ": " << std::endl;
			std::cout << pose.matrix() << std::endl;
		}
		boost::this_thread::sleep( boost::posix_time::milliseconds(1000) );
	}
	return 0;
}

int main(int argc, char** argv)
{
	ROS_INFO("Tracker Started.");
	ros::init(argc, argv, "simple_tracker");

	ros::NodeHandle nh;
	ros::ServiceClient poseClient = nh.serviceClient<HuboApplication::SetHuboArmPose>("/hubo/set_arm");

	ros::Rate loop_rate(0.1);

	while (ros::ok())
	{
		/*double x = randbetween(X_MIN, X_MAX);
		double y = randbetween(Y_MIN, Y_MAX);
		double z = randbetween(Z_MIN, Z_MAX);*/

		Eigen::Isometry3d ePose;
		tf::StampedTransform tPose;
		HuboApplication::SetHuboArmPose srv;

		ePose.matrix() <<   1,  0,  0, .3,
							0,  1,  0, .2,
							0,  0,  1,  0,
							0,  0,  0,  1;

		ePose.rotate(Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ()));
		ePose.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
/*
		Collision_Checker cc;
		cc.initCollisionChecker();
		cc.checkSelfCollision(ePose);
		tf::TransformEigenToTF(ePose, tPose);

		tf::poseTFToMsg(tPose, srv.request.Target);
		srv.request.ArmIndex = 1;
		poseClient.call(srv);
*/
		ros::spinOnce();
		loop_rate.sleep();
	}

	ros::spin();

	return 0;
}