Пример #1
0
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;
}
Пример #2
0
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;
    }
    
}
Пример #3
0
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;
}
Пример #4
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 );
    }


}
Пример #5
0
//==============================================================================
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;
}
Пример #6
0
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);
}
Пример #7
0
	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;
	}
Пример #8
0
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;
}
Пример #9
0
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;
}
Пример #10
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));
}
Пример #11
0
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;
}
Пример #12
0
//==============================================================================
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);
}
Пример #14
0
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;
}
Пример #16
0
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ){
	PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb,newFrame.depth,camera);
	
	// 合并点云
	PointCloud::Ptr output(new PointCloud());
	pcl::transformPointCloud(*original,*output,T.matrix());
	*newCloud += *output;

	// Voxel grid 滤波降采样
	static pcl::VoxelGrid<PointT> voxel;
	static ParameterReader pd;
	double gridsize = atof(pd.getData("voxel_grid").c_str());
	voxel.setLeafSize(gridsize,gridsize,gridsize);
	voxel.setInputCloud(newCloud);
	PointCloud::Ptr tmp(new PointCloud());
	voxel.filter(*tmp);
	return tmp;
}
Пример #17
0
Mapper::PointCloud::Ptr Mapper::generatePointCloud(const RGBDFrame::Ptr &frame)
{
    PointCloud::Ptr tmp( new PointCloud() );
    if ( frame->pointcloud == nullptr )
    {
        // point cloud is null ptr
        frame->pointcloud = boost::make_shared<PointCloud>();
#pragma omp parallel for
        for ( int m=0; m<frame->depth.rows; m+=3 )
        {
            for ( int n=0; n<frame->depth.cols; n+=3 )
            {
                ushort d = frame->depth.ptr<ushort>(m)[n];
                if (d == 0)
                    continue;
                if (d > max_distance * frame->camera.scale)
                    continue;
                PointT p;
                cv::Point3f p_cv = frame->project2dTo3d(n, m);
                p.b = frame->rgb.ptr<uchar>(m)[n*3];
                p.g = frame->rgb.ptr<uchar>(m)[n*3+1];
                p.r = frame->rgb.ptr<uchar>(m)[n*3+2];

                p.x = p_cv.x;
                p.y = p_cv.y;
                p.z = p_cv.z;

                frame->pointcloud->points.push_back( p );
            }
        }
    }

    Eigen::Isometry3d T = frame->getTransform().inverse();
    pcl::transformPointCloud( *frame->pointcloud, *tmp, T.matrix());
    tmp->is_dense = false;
    return tmp;
}
Пример #18
0
void
pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
  bool make_global,
  const Eigen::Isometry3d & pose)
{
  // TODO: check if this works for for rows/cols >1  and for width&height != 640x480
  // i.e. multiple tiled images
  pc->width    = col_width_;
  pc->height   = row_height_;
  // Was:
  //pc->width    = camera_width_;
  //pc->height   = camera_height_;

  pc->is_dense = false;
  pc->points.resize (pc->width*pc->height);

  int points_added = 0;

  float camera_fx_reciprocal_ = 1.0f / camera_fx_;
  float camera_fy_reciprocal_ = 1.0f / camera_fy_;
  float zn = z_near_;
  float zf = z_far_;

  const uint8_t* color_buffer = getColorBuffer();

  // TODO: support decimation
  // Copied the format of RangeImagePlanar::setDepthImage()
  // Use this as a template for decimation
  for (int y = 0; y < row_height_ ; ++y) //camera_height_
  {
    for (int x = 0; x < col_width_ ; ++x)  // camera_width_
    {
      // Find XYZ from normalized 0->1 mapped disparity
      int idx = points_added; // y*camera_width_ + x;
      float d = depth_buffer_[y*camera_width_ + x] ;
      if (d < 1.0) // only add points with depth buffer less than max (20m) range
      {
        float z = zf*zn/((zf-zn)*(d - zf/(zf-zn)));

        // TODO: add mode to ignore points with no return i.e. depth_buffer_ ==1
        // NB: OpenGL uses a Right Hand system with +X right, +Y up, +Z back out of the screen,
        // The Z-buffer is natively -1 (far) to 1 (near)
        // But in this class we invert this to be 0 (near, 0.7m) and 1 (far, 20m)
        // ... so by negating y we get to a right-hand computer vision system
        // which is also used by PCL and OpenNi
        pc->points[idx].z = z;
        pc->points[idx].x = (static_cast<float> (x)-camera_cx_) * z * (-camera_fx_reciprocal_);
        pc->points[idx].y = (static_cast<float> (y)-camera_cy_) * z * (-camera_fy_reciprocal_);

	int rgb_idx = y*col_width_ + x;  //camera_width_
        pc->points[idx].b = color_buffer[rgb_idx*3+2]; // blue
        pc->points[idx].g = color_buffer[rgb_idx*3+1]; // green
        pc->points[idx].r = color_buffer[rgb_idx*3]; // red
        points_added++;
      }
    }
  }
  pc->width    = 1;
  pc->height   = points_added;
  pc->points.resize (points_added);

  if (make_global)
  {
    // Go from OpenGL to (Z-up, X-forward, Y-left)
    Eigen::Matrix4f T;
    T <<  0, 0, -1, 0,
         -1, 0,  0, 0,
          0, 1,  0, 0,
          0, 0,  0, 1;
    Eigen::Matrix4f m = pose.matrix ().cast<float> ();
    m = m * T;
    pcl::transformPointCloud (*pc, *pc, m);
  }
  else
  {
    // Go from OpenGL to Camera (Z-forward, X-right, Y-down)
    Eigen::Matrix4f T;
    T <<  1,  0,  0, 0,
          0, -1,  0, 0,
          0,  0, -1, 0,
          0,  0,  0, 1;
    pcl::transformPointCloud (*pc, *pc, T);

    // Go from Camera to body (Z-up, X-forward, Y-left)
    Eigen::Matrix4f cam_to_body;
    cam_to_body <<  0,  0, 1, 0,
                   -1,  0, 0, 0,
                    0, -1, 0, 0,
                    0,  0, 0, 1;
    Eigen::Matrix4f camera = pose.matrix ().cast<float> ();
    camera = camera * cam_to_body;
    pc->sensor_origin_ = camera.rightCols (1);
    Eigen::Quaternion<float> quat (camera.block<3,3> (0,0));
    pc->sensor_orientation_ = quat;
  }
}
Пример #19
0
int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"用法:./direct_sparse  path_to_dataset"<<endl;
        return 1;
    }
    srand ( ( unsigned int ) time ( 0 ) );//随机数
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    ifstream fin ( associate_file );

    string rgb_file, depth_file, time_rgb, time_depth;
    	//rgb图像对应时间 rgb图像 深度图像对应时间 深度图像
    cv::Mat color, depth, gray;// 彩色图 深度图  灰度图
    vector<Measurement> measurements;
    // 相机内参
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;// mm  变成 m  
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();//相机位姿 [R t] 的齐次表示 4*4

    cv::Mat prev_color;
    // 我们以第一个图像为参考,对后续图像和参考图像做直接法
    for ( int index=0; index<10; index++ )
    {
        cout<<"*********** loop "<<index<<" ************"<<endl;
        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
        color = cv::imread ( path_to_dataset+"/"+rgb_file );// rgb 图像
        depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );// 深度图
        if ( color.data==nullptr || depth.data==nullptr )
            continue; 
        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );//彩色图到灰度图
	
        if ( index ==0 )//第一帧
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect ( color, keypoints );//检测 特征点
            for ( auto kp:keypoints )
            {
                // 去掉邻近图像边缘处的点
                if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;//跳过以下
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//对于特征点的深度
                if ( d==0 )
                    continue;//跳过
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );//2D像素坐标   转换成 相机坐标系下的 三维点 3D
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );//特征点 对应的灰度值   坐标值为整数 需要取整
                measurements.push_back ( Measurement ( p3d, grayscale ) );//测量值为 三维点 和 对应图像的灰度值
            }
            prev_color = color.clone();//赋值 图像
            continue;//第一幅图 跳过 以下
        }
        // 使用直接法计算相机运动
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//计时开始
        poseEstimationDirect ( measurements, &gray, K, Tcw );//测量值
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();//计时结束
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"直接法耗时 direct method costs time: "<<time_used.count() <<" seconds."<<endl;
        cout<<"转换矩阵 Tcw="<<Tcw.matrix() <<endl;

        // 画特征点 plot the feature points
        cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
        prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
        color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
        for ( Measurement m:measurements )
        {
            if ( rand() > RAND_MAX/5 )
                continue;
            Eigen::Vector3d p = m.pos_world;//测量值的 三维点 p
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//转换成 2d像素坐标
            Eigen::Vector3d p2 = Tcw*m.pos_world;//变换到 第二帧图像的坐标系下   
            Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//转化成 2d像素坐标
            if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )// 超出范围的 跳过
                continue;

            float b = 255*float ( rand() ) /RAND_MAX;//随机颜色 分量
            float g = 255*float ( rand() ) /RAND_MAX;
            float r = 255*float ( rand() ) /RAND_MAX;
            cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
        }
        cv::imshow ( "result", img_show );
        cv::waitKey ( 0 );//等待按键

    }
    return 0;
}
Пример #20
0
TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS)
{
    int numTest = 100;

    // AdT(V) == T * V * InvT
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector6d V = Eigen::Vector6d::Random();

        Eigen::Vector6d AdTV = AdT(T, V);

        // Ad(T, V) = T * [V] * InvT
        Eigen::Matrix4d T_V_InvT = T.matrix() * toMatrixForm(V) * T.inverse().matrix();
        Eigen::Vector6d T_V_InvT_se3 = fromMatrixForm(T_V_InvT);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), T_V_InvT_se3(j), LIE_GROUP_OPT_TOL);

        // Ad(T, V) = [R 0; [p]R R] * V
        Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero();
        AdTMatrix.topLeftCorner<3,3>() = T.linear();
        AdTMatrix.bottomRightCorner<3,3>() = T.linear();
        AdTMatrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(T.translation()) * T.linear();
        Eigen::Vector6d AdTMatrix_V = AdTMatrix * V;
        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTMatrix_V(j), LIE_GROUP_OPT_TOL);
    }

    // AdR == AdT([R 0; 0 1], V)
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
        R = T.linear();
        Eigen::Vector6d V = Eigen::Vector6d::Random();

        Eigen::Vector6d AdTV = AdT(R, V);
        Eigen::Vector6d AdRV = AdR(T, V);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdRV(j), LIE_GROUP_OPT_TOL);
    }

    // AdTAngular == AdT(T, se3(w, 0))
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector3d w = Eigen::Vector3d::Random();
        Eigen::Vector6d V = Eigen::Vector6d::Zero();
        V.head<3>() = w;

        Eigen::Vector6d AdTV = AdT(T, V);
        Eigen::Vector6d AdTAng = AdTAngular(T, w);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTAng(j), LIE_GROUP_OPT_TOL);
    }

    // AdTLinear == AdT(T, se3(w, 0))
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector3d v = Eigen::Vector3d::Random();
        Eigen::Vector6d V = Eigen::Vector6d::Zero();
        V.tail<3>() = v;

        Eigen::Vector6d AdTV = AdT(T, V);
        Eigen::Vector6d AdTLin = AdTLinear(T, v);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
    }

    // AdTJac
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector3d v = Eigen::Vector3d::Random();
        Eigen::Vector6d V = Eigen::Vector6d::Zero();
        V.tail<3>() = v;

        Eigen::Vector6d AdTV = AdT(T, V);
        Eigen::Vector6d AdTLin = AdTLinear(T, v);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
    }

    // AdInvT
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Isometry3d InvT = T.inverse();
        Eigen::Vector6d V = Eigen::Vector6d::Random();

        Eigen::Vector6d Ad_InvT = AdT(InvT, V);
        Eigen::Vector6d AdInv_T = AdInvT(T, V);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(Ad_InvT(j), AdInv_T(j), LIE_GROUP_OPT_TOL);
    }

    // AdInvRLinear
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector3d v = Eigen::Vector3d::Random();
        Eigen::Vector6d V = Eigen::Vector6d::Zero();
        V.tail<3>() = v;
        Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
        R = T.linear();

        Eigen::Vector6d AdT_ = AdT(R.inverse(), V);
        Eigen::Vector6d AdInvRLinear_ = AdInvRLinear(T, v);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdT_(j), AdInvRLinear_(j), LIE_GROUP_OPT_TOL);
    }

    // dAdT
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector6d F = Eigen::Vector6d::Random();

        Eigen::Vector6d dAdTF = dAdT(T, F);

        // dAd(T, F) = [R 0; [p]R R]^T * F
        Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero();
        AdTMatrix.topLeftCorner<3,3>() = T.linear();
        AdTMatrix.bottomRightCorner<3,3>() = T.linear();
        AdTMatrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(T.translation()) * T.linear();
        Eigen::Vector6d AdTTransMatrix_V = AdTMatrix.transpose() * F;
        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(dAdTF(j), AdTTransMatrix_V(j), LIE_GROUP_OPT_TOL);
    }

    // dAdInvT
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Isometry3d InvT = T.inverse();
        Eigen::Vector6d F = Eigen::Vector6d::Random();

        Eigen::Vector6d dAdInvT_F = dAdInvT(T, F);

        //
        Eigen::Vector6d dAd_InvTF = dAdT(InvT, F);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(dAdInvT_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL);

        // dAd(T, F) = [R 0; [p]R R]^T * F
        Eigen::Matrix6d AdInvTMatrix = Eigen::Matrix6d::Zero();
        AdInvTMatrix.topLeftCorner<3,3>() = InvT.linear();
        AdInvTMatrix.bottomRightCorner<3,3>() = InvT.linear();
        AdInvTMatrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(InvT.translation()) * InvT.linear();
        Eigen::Vector6d AdInvTTransMatrix_V = AdInvTMatrix.transpose() * F;
        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(dAdInvT_F(j), AdInvTTransMatrix_V(j), LIE_GROUP_OPT_TOL);
    }

    // dAdInvR
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Isometry3d InvT = T.inverse();
        Eigen::Isometry3d InvR = Eigen::Isometry3d::Identity();
        InvR = InvT.linear();
        Eigen::Vector6d F = Eigen::Vector6d::Random();

        Eigen::Vector6d dAdInvR_F = dAdInvR(T, F);

        //
        Eigen::Vector6d dAd_InvTF = dAdT(InvR, F);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(dAdInvR_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL);
    }

    // ad
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d V = Eigen::Vector6d::Random();
        Eigen::Vector6d W = Eigen::Vector6d::Random();

        Eigen::Vector6d ad_V_W = ad(V, W);

        //
        Eigen::Matrix6d adV_Matrix = Eigen::Matrix6d::Zero();
        adV_Matrix.topLeftCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
        adV_Matrix.bottomRightCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
        adV_Matrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(V.tail<3>());
        Eigen::Vector6d adV_Matrix_W = adV_Matrix * W;

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(ad_V_W(j), adV_Matrix_W(j), LIE_GROUP_OPT_TOL);
    }

    // dad
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d V = Eigen::Vector6d::Random();
        Eigen::Vector6d F = Eigen::Vector6d::Random();

        Eigen::Vector6d dad_V_F = dad(V, F);

        //
        Eigen::Matrix6d dadV_Matrix = Eigen::Matrix6d::Zero();
        dadV_Matrix.topLeftCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
        dadV_Matrix.bottomRightCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
        dadV_Matrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(V.tail<3>());
        Eigen::Vector6d dadV_Matrix_F= dadV_Matrix.transpose() * F;

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(dad_V_F(j), dadV_Matrix_F(j), LIE_GROUP_OPT_TOL);
    }
}
const std::shared_ptr< std::vector< CLandmark* > > CTrackerStereoMotionModel::_getNewLandmarks( const UIDFrame& p_uFrame,
                                                                                     cv::Mat& p_matDisplay,
                                                                                     const cv::Mat& p_matImageLEFT,
                                                                                     const cv::Mat& p_matImageRIGHT,
                                                                                     const Eigen::Isometry3d& p_matTransformationWORLDtoLEFT,
                                                                                     const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD,
                                                                                     const Eigen::Vector3d& p_vecRotation )
{
    //ds precompute extrinsics
    const MatrixProjection matProjectionWORLDtoLEFT( m_pCameraLEFT->m_matProjection*p_matTransformationWORLDtoLEFT.matrix( ) );

    //ds solution holder
    std::shared_ptr< std::vector< CLandmark* > > vecNewLandmarks( std::make_shared< std::vector< CLandmark* > >( ) );

    //ds detect new keypoints
    //const std::shared_ptr< std::vector< cv::KeyPoint > > vecKeyPoints( m_cDetector.detectKeyPointsTilewise( p_matImageLEFT, matMask ) );
    std::vector< cv::KeyPoint > vecKeyPoints;
    m_pDetector->detect( p_matImageLEFT, vecKeyPoints, m_cMatcher.getMaskActiveLandmarks( p_matTransformationWORLDtoLEFT, p_matDisplay ) );

    //ds compute descriptors for the keypoints
    CDescriptor matReferenceDescriptors;
    //m_pExtractor->compute( p_matImageLEFT, *vecKeyPoints, matReferenceDescriptors );
    m_pExtractor->compute( p_matImageLEFT, vecKeyPoints, matReferenceDescriptors );

    //ds process the keypoints and see if we can use them as landmarks
    for( uint32_t u = 0; u < vecKeyPoints.size( ); ++u )
    {
        //ds current points
        const cv::KeyPoint cKeyPointLEFT( vecKeyPoints[u] );
        const cv::Point2f ptLandmarkLEFT( cKeyPointLEFT.pt );
        const CDescriptor matDescriptorLEFT( matReferenceDescriptors.row(u) );

        try
        {
            //ds triangulate the point
            const CMatchTriangulation cMatch( m_pTriangulator->getPointTriangulatedCompactInRIGHT( p_matImageRIGHT, cKeyPointLEFT, matDescriptorLEFT ) );
            const CPoint3DCAMERA vecPointTriangulatedLEFT( cMatch.vecPointXYZCAMERA );
            const CDescriptor matDescriptorRIGHT( cMatch.matDescriptorCAMERA );

            //ds check depth
            const double dDepthMeters( vecPointTriangulatedLEFT.z( ) );

            //ds check if point is in front of camera an not more than a defined distance away
            if( m_dMinimumDepthMeters < dDepthMeters && m_dMaximumDepthMeters > dDepthMeters )
            {
                //ds compute triangulated point in world frame
                const CPoint3DWORLD vecPointTriangulatedWORLD( p_matTransformationLEFTtoWORLD*vecPointTriangulatedLEFT );

                //ds landmark right
                const cv::Point2f ptLandmarkRIGHT( cMatch.ptUVCAMERA );

                //ds allocate a new landmark and add the current position
                CLandmark* pLandmark( new CLandmark( m_uAvailableLandmarkID,
                                                     matDescriptorLEFT,
                                                     cMatch.matDescriptorCAMERA,
                                                     cKeyPointLEFT.size,
                                                     vecPointTriangulatedWORLD,
                                                     m_pCameraLEFT->getNormalHomogenized( ptLandmarkLEFT ),
                                                     ptLandmarkLEFT,
                                                     ptLandmarkRIGHT,
                                                     vecPointTriangulatedLEFT,
                                                     p_matTransformationLEFTtoWORLD.translation( ),
                                                     p_vecRotation,
                                                     matProjectionWORLDtoLEFT,
                                                     p_uFrame ) );

                //ds log creation
                CLogger::CLogLandmarkCreation::addEntry( p_uFrame, pLandmark, dDepthMeters, ptLandmarkLEFT, ptLandmarkRIGHT );

                //ds add to newly detected
                vecNewLandmarks->push_back( pLandmark );

                //ds next landmark id
                ++m_uAvailableLandmarkID;

                //ds draw detected point
                cv::line( p_matDisplay, ptLandmarkLEFT, cv::Point2f( ptLandmarkLEFT.x+m_pCameraSTEREO->m_uPixelWidth, ptLandmarkLEFT.y ), CColorCodeBGR( 175, 175, 175 ) );
                cv::circle( p_matDisplay, ptLandmarkLEFT, 2, CColorCodeBGR( 0, 255, 0 ), -1 );
                //cv::circle( p_matDisplay, ptLandmarkLEFT, cLandmark->dKeyPointSize, CColorCodeBGR( 255, 0, 0 ), 1 );
                cv::putText( p_matDisplay, std::to_string( pLandmark->uID ) , cv::Point2d( ptLandmarkLEFT.x+pLandmark->dKeyPointSize, ptLandmarkLEFT.y+pLandmark->dKeyPointSize ), cv::FONT_HERSHEY_PLAIN, 0.5, CColorCodeBGR( 0, 0, 255 ) );

                //ds draw reprojection of triangulation
                cv::circle( p_matDisplay, cv::Point2d( ptLandmarkRIGHT.x+m_pCameraSTEREO->m_uPixelWidth, ptLandmarkRIGHT.y ), 2, CColorCodeBGR( 255, 0, 0 ), -1 );
            }
            else
            {
                cv::circle( p_matDisplay, ptLandmarkLEFT, 2, CColorCodeBGR( 0, 0, 255 ), -1 );
                //cv::circle( p_matDisplay, ptLandmarkLEFT, cKeyPoint.size, CColorCodeBGR( 0, 0, 255 ) );

                //std::printf( "<CTrackerStereoMotionModel>(_getNewLandmarks) could not find match for keypoint (invalid depth: %f m)\n", vecPointTriangulatedLEFT(2) );
            }
        }
        catch( const CExceptionNoMatchFound& p_cException )
        {
            cv::circle( p_matDisplay, ptLandmarkLEFT, 2, CColorCodeBGR( 0, 0, 255 ), -1 );
            //cv::circle( p_matDisplay, ptLandmarkLEFT, cKeyPoint.size, CColorCodeBGR( 0, 0, 255 ) );

            //std::printf( "<CTrackerStereoMotionModel>(_getNewLandmarks) could not find match for keypoint (%s)\n", p_cException.what( ) );
        }
    }

    //std::printf( "<CTrackerStereoMotionModel>(_getNewLandmarks) added new landmarks: %lu/%lu\n", vecNewLandmarks->size( ), vecKeyPoints.size( ) );

    //ds return found landmarks
    return vecNewLandmarks;
}
Пример #22
0
int main( int argc, char** argv )
{
    // 调用格式:命令 [第一个图] [第二个图]
    if (argc != 3)
    {
        cout<<"Usage: ba_example img1, img2"<<endl;
        exit(1);
    }
    
    // 读取图像
    cv::Mat img1 = cv::imread( argv[1] ); 
    cv::Mat img2 = cv::imread( argv[2] ); 
    
    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
    {
        cout<<"匹配点不够!"<<endl;
        return 0;
    }
    cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降 
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );
    
    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );
    
    // 添加节点
    // 两个位姿节点
    for ( int i=0; i<2; i++ )
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose,因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道,只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx; 
        double y = ( pts1[i].y - cy ) * z / fy; 
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }
    
    // 准备相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );
    
    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    
    cout<<"开始优化"<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"优化完毕"<<endl;
    
    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;
    
    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }
    
    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大,说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        {
            cout<<"error = "<<e->chi2()<<endl;
        }
        else 
        {
            inliers++;
        }
    }
    
    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");
    return 0;
}
Пример #23
0
void GraphicEnd::lostRecovery()
{
    //以present作为新的关键帧
    cout<<BOLDYELLOW<<"Lost Recovery..."<<RESET<<endl;
    //把present中的数据存储到current中
    _currKF.id ++;
    _currKF.planes = _present.planes;
    _currKF.frame_index = _index;
    _lastRGB = _currRGB.clone();
    _kf_pos = _robot;

    //waitKey(0);
    _keyframes.push_back( _currKF );
    
    //将current关键帧存储至全局优化器
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //顶点
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    else
        v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );

    //由于当前位置不知道,所以不添加与上一帧相关的边
    if (_use_odometry)
    {
        Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
        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) = information(1, 1) = 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;
        _lost = 0;
        return;
    }
    //check loop closure
    for (int i=0; i<_keyframes.size() - 1; i++)
    {
        vector<PLANE>& p1 = _keyframes[ i ].planes;
        Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
        
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        //若匹配上,则在两个帧之间加一条边
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[i].id );
        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) = 100; 
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }
    
}
Пример #24
0
//回环检测:在过去的帧中随机取_loopclosure_frames那么多帧进行两两比较
void GraphicEnd::loopClosure()
{
    if (_keyframes.size() <= 3 )  //小于3时,回环没有意义
        return;
    cout<<"Checking loop closure."<<endl;
    waitKey(10);
    vector<int> checked;
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;

    //相邻帧
    for (int i=-3; i>-5; i--)
    {
        int n = _keyframes.size() + i;
        if (n>=0)
        {
            vector<PLANE>& p1 = _keyframes[n].planes;
            Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
            if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
                continue;
            T = T.inverse();
            //若匹配上,则在两个帧之间加一条边
            EdgeSE3* edge = new EdgeSE3();
            edge->vertices() [0] = opt.vertex( _keyframes[n].id );
            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 );
            edge->setRobustKernel( _pSLAMEnd->_robustKernel );
            opt.addEdge( edge );
        }
        else
            break;
    }
    //搜索种子帧
    cout<<"checking seeds, seed.size()"<<_seed.size()<<endl;
    vector<int> newseed;
    for (size_t i=0; i<_seed.size(); i++)
    {
        vector<PLANE>& p1 = _keyframes[_seed[i]].planes;
        Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        //若匹配上,则在两个帧之间加一条边
        checked.push_back( _seed[i] );
        newseed.push_back( _seed[i] );
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[_seed[i]].id );
        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 );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }

    //随机搜索
    cout<<"checking random frames"<<endl;
    for (int i=0; i<_loopclosure_frames; i++)
    {
        int frame = rand() % (_keyframes.size() -3 ); //随机在过去的帧中取一帧
        if ( find(checked.begin(), checked.end(), frame) != checked.end() ) //之前已检查过
            continue;
        checked.push_back( frame );
        vector<PLANE>& p1 = _keyframes[frame].planes;
        RESULT_OF_MULTIPNP result = multiPnP( p1, _currKF.planes, true, _keyframes[frame].frame_index, 20 );
        Eigen::Isometry3d T = result.T;
        
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        displayLC( _keyframes[frame].frame_index, _currKF.frame_index, result.norm);
        newseed.push_back( frame );
        //若匹配上,则在两个帧之间加一条边
        cout<<BOLDBLUE<<"find a loop closure between kf "<<_currKF.id<<" and kf "<<frame<<RESET<<endl;
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[frame].id );
        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 );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }

    waitKey(10);
    _seed = newseed;
}
bool G2oSlamInterface::addEdge(const std::string& tag, int id, int dimension, int v1Id, int v2Id, const std::vector<double>& measurement, const std::vector<double>& information)
{
  (void) tag;
  (void) id;
  size_t oldEdgesSize = _optimizer->edges().size();

  if (dimension == 3) {

    SE2 transf(measurement[0], measurement[1], measurement[2]);
    Eigen::Matrix3d infMat;
    int idx = 0;
    for (int r = 0; r < 3; ++r)
      for (int c = r; c < 3; ++c, ++idx) {
        assert(idx < (int)information.size());
        infMat(r,c) = infMat(c,r) = information[idx];
      }
    //cerr << PVAR(infMat) << endl;

    int doInit = 0;
    SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
    SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
    if (! v1) {
      OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
      _verticesAdded.insert(v);
      doInit = 1;
      ++_nodesAdded;
    }

    if (! v2) {
      OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
      _verticesAdded.insert(v);
      doInit = 2;
      ++_nodesAdded;
    }

    if (_optimizer->edges().size() == 0) {
      cerr << "FIRST EDGE ";
      if (v1->id() < v2->id()) {
        cerr << "fixing " << v1->id() << endl;
        v1->setFixed(true);
      }
      else {
        cerr << "fixing " << v2->id() << endl;
        v2->setFixed(true);
      }
    }

    OnlineEdgeSE2* e = new OnlineEdgeSE2;
    e->vertices()[0] = v1;
    e->vertices()[1] = v2;
    e->setMeasurement(transf);
    e->setInformation(infMat);
    _optimizer->addEdge(e);
    _edgesAdded.insert(e);

    if (doInit) {
      OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
      OptimizableGraph::Vertex* to   = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
      switch (doInit){
        case 1: // initialize v1 from v2
          {
            HyperGraph::VertexSet toSet;
            toSet.insert(to);
            if (e->initialEstimatePossible(toSet, from) > 0.) {
              e->initialEstimate(toSet, from);
            }
            break;
          }
        case 2: 
          {
            HyperGraph::VertexSet fromSet;
            fromSet.insert(from);
            if (e->initialEstimatePossible(fromSet, to) > 0.) {
              e->initialEstimate(fromSet, to);  
            }
            break;
          }
        default: cerr << "doInit wrong value\n"; 
      }
    }

  }
  else if (dimension == 6) {

    Eigen::Isometry3d transf;
    Matrix<double, 6, 6> infMat;

    if (measurement.size() == 7) { // measurement is a Quaternion
      Vector7d meas;
      for (int i=0; i<7; ++i) 
        meas(i) = measurement[i];
      // normalize the quaternion to recover numerical precision lost by storing as human readable text
      Vector4d::MapType(meas.data()+3).normalize();
      transf = internal::fromVectorQT(meas);

      for (int i = 0, idx = 0; i < infMat.rows(); ++i)
        for (int j = i; j < infMat.cols(); ++j){
          infMat(i,j) = information[idx++];
          if (i != j)
            infMat(j,i)=infMat(i,j);
        }
    } else { // measurement consists of Euler angles
      Vector6d aux;
      aux << measurement[0], measurement[1], measurement[2],measurement[3], measurement[4], measurement[5];
      transf = internal::fromVectorET(aux);
      Matrix<double, 6, 6> infMatEuler;
      int idx = 0;
      for (int r = 0; r < 6; ++r)
        for (int c = r; c < 6; ++c, ++idx) {
          assert(idx < (int)information.size());
          infMatEuler(r,c) = infMatEuler(c,r) = information[idx];
        }
      // convert information matrix to our internal representation
      Matrix<double, 6, 6> J;
      SE3Quat transfAsSe3(transf.matrix().topLeftCorner<3,3>(), transf.translation());
      jac_quat3_euler3(J, transfAsSe3);
      infMat.noalias() = J.transpose() * infMatEuler * J;
      //cerr << PVAR(transf.matrix()) << endl;
      //cerr << PVAR(infMat) << endl;
    }

    int doInit = 0;
    SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
    SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
    if (! v1) {
      OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
      _verticesAdded.insert(v);
      doInit = 1;
      ++_nodesAdded;
    }

    if (! v2) {
      OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
      _verticesAdded.insert(v);
      doInit = 2;
      ++_nodesAdded;
    }

    if (_optimizer->edges().size() == 0) {
      cerr << "FIRST EDGE ";
      if (v1->id() < v2->id()) {
        cerr << "fixing " << v1->id() << endl;
        v1->setFixed(true);
      }
      else {
        cerr << "fixing " << v2->id() << endl;
        v2->setFixed(true);
      }
    }

    OnlineEdgeSE3* e = new OnlineEdgeSE3;
    e->vertices()[0] = v1;
    e->vertices()[1] = v2;
    e->setMeasurement(transf);
    e->setInformation(infMat);
    _optimizer->addEdge(e);
    _edgesAdded.insert(e);

    if (doInit) {
      OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
      OptimizableGraph::Vertex* to   = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
      switch (doInit){
        case 1: // initialize v1 from v2
          {
            HyperGraph::VertexSet toSet;
            toSet.insert(to);
            if (e->initialEstimatePossible(toSet, from) > 0.) {
              e->initialEstimate(toSet, from);
            }
            break;
          }
        case 2: 
          {
            HyperGraph::VertexSet fromSet;
            fromSet.insert(from);
            if (e->initialEstimatePossible(fromSet, to) > 0.) {
              e->initialEstimate(fromSet, to);  
            }
            break;
          }
        default: cerr << "doInit wrong value\n"; 
      }
    }

  }
  else {
    cerr << __PRETTY_FUNCTION__ << " not implemented for this dimension" << endl;
    return false;
  }

  if (oldEdgesSize == 0) {
    _optimizer->jacobianWorkspace().allocate();
  }

  return true;
}
Пример #26
0
int GraphicEnd::run()
{
    //清空present并读取新的数据
    cout<<"********************"<<endl;
    _present.planes.clear();
    
    readimage();
    
    //处理present
    _present.planes = extractPlanesAndGenerateImage( _currCloud, _currRGB, _currDep );
    //_present.planes = extractPlanes( _currCloud );
    //generateImageOnPlane( _currRGB, _present.planes, _currDep );

    for ( size_t i=0; i<_present.planes.size(); i++ )
    {
        _present.planes[i].kp = extractKeypoints( _present.planes[i].image );
        _present.planes[i].desp = extractDescriptor( _currRGB, _present.planes[i].kp );
        compute3dPosition( _present.planes[i], _currDep);
    }

    // 求解present到current的变换矩阵
    RESULT_OF_MULTIPNP result = multiPnP( _currKF.planes, _present.planes );
    Eigen::Isometry3d T = result.T;
    T = T.inverse();  //好像是反着的
    
    // 如果平移和旋转超过一个阈值,则定义新的关键帧
    if ( T.matrix() == Eigen::Isometry3d::Identity().matrix() )
    {
        //未匹配上
        cout<<BOLDRED"This frame 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位置
        _robot = T * _kf_pos;
        _lost = 0;
    }
    if (_lost > _lost_frames)
    {
        cerr<<"the robot lost. Perform lost recovery."<<endl;
        lostRecovery();
    }


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

    return 1;
}
Пример #27
0
void test_relative_values(bool spatial_targets, bool spatial_followers)
{
  const double tolerance = 1e-8;

  // These frames will form a chain
  SimpleFrame A(Frame::World(), "A");
  SimpleFrame B(&A, "B");
  SimpleFrame C(&B, "C");
  SimpleFrame D(&C, "D");

  std::vector<SimpleFrame*> targets;
  targets.push_back(&A);
  targets.push_back(&B);
  targets.push_back(&C);
  targets.push_back(&D);

  // R will be an arbitrary reference frame
  SimpleFrame R(Frame::World(), "R");
  targets.push_back(&R);

  // Each of these frames will attempt to track one of the frames in the chain
  // with respect to the frame R.
  SimpleFrame AR(&R, "AR");
  SimpleFrame BR(&R, "BR");
  SimpleFrame CR(&R, "CR");
  SimpleFrame DR(&R, "DR");
  SimpleFrame RR(&R, "RR");

  std::vector<SimpleFrame*> followers;
  followers.push_back(&AR);
  followers.push_back(&BR);
  followers.push_back(&CR);
  followers.push_back(&DR);
  followers.push_back(&RR);

  assert( targets.size() == followers.size() );

  randomize_target_values(targets, spatial_targets);
  set_relative_values(targets, followers, spatial_followers);
  check_world_values(targets, followers, tolerance);

  // Check every combination of relative values
  for(std::size_t i=0; i<targets.size(); ++i)
  {
    Frame* T = targets[i];
    for(std::size_t j=0; j<followers.size(); ++j)
    {
      Frame* F = followers[j];

      check_values(targets, followers, T, F, tolerance);
      check_values(targets, followers, F, T, tolerance);
      check_offset_computations(targets, followers, T, F, tolerance);
    }
  }

  // Test SimpleFrame::setTransform()
  for(std::size_t i=0; i<followers.size(); ++i)
  {
    for(std::size_t j=0; j<followers.size(); ++j)
    {
      SimpleFrame* F = followers[i];
      SimpleFrame* T = followers[j];
      Eigen::Isometry3d tf;
      randomize_transform(tf, 1, 2*M_PI);
      T->setTransform(tf, F);
      if(i != j)
        EXPECT_TRUE( equals(T->getTransform(F).matrix(), tf.matrix(), 1e-10));
    }
  }
}
Пример #28
0
TEST(FRAMES, FORWARD_KINEMATICS_CHAIN)
{
  std::vector<SimpleFrame*> frames;

  double tolerance = 1e-6;

  SimpleFrame A(Frame::World(), "A");
  frames.push_back(&A);
  SimpleFrame B(&A, "B");
  frames.push_back(&B);
  SimpleFrame C(&B, "C");
  frames.push_back(&C);
  SimpleFrame D(&C, "D");
  frames.push_back(&D);

  // -- Test Position --------------------------------------------------------
  EXPECT_TRUE( equals(D.getTransform().matrix(),
                      Eigen::Isometry3d::Identity().matrix(),
                      tolerance));

  Eigen::aligned_vector<Eigen::Isometry3d> tfs;
  tfs.resize(frames.size(), Eigen::Isometry3d::Identity());

  randomize_transforms(tfs);

  for(std::size_t i=0; i<frames.size(); ++i)
  {
    SimpleFrame* F = frames[i];
    F->setRelativeTransform(tfs[i]);
  }

  for(std::size_t i=0; i<frames.size(); ++i)
  {
    Frame* F = frames[i];
    Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
    for(std::size_t j=0; j<=i; ++j)
    {
      expectation = expectation * tfs[j];
    }

    Eigen::Isometry3d actual = F->getTransform();
    EXPECT_TRUE( equals(actual.matrix(), expectation.matrix(), tolerance));
  }

  randomize_transforms(tfs);
  for(std::size_t i=0; i<frames.size(); ++i)
  {
    SimpleFrame* F = frames[i];
    F->setRelativeTransform(tfs[i]);
  }

  Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
  for(std::size_t j=0; j<frames.size(); ++j)
    expectation = expectation * tfs[j];

  EXPECT_TRUE( equals(frames.back()->getTransform().matrix(),
                      expectation.matrix(),
                      tolerance) );


  // -- Test Velocity --------------------------------------------------------

  // Basic forward spatial velocity propagation
  { // The brackets are to allow reusing variable names
    Eigen::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
    Eigen::aligned_vector<Eigen::Vector6d> v_total(frames.size());

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<6>();

      SimpleFrame* F = frames[i];
      F->setRelativeSpatialVelocity(v_rels[i]);

      if(i>0)
        compute_spatial_velocity(v_total[i-1], v_rels[i],
            F->getRelativeTransform(), v_total[i]);
      else
        compute_spatial_velocity(Eigen::Vector6d::Zero(), v_rels[i],
                                 F->getRelativeTransform(), v_total[i]);
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];

      Eigen::Vector6d v_actual = F->getSpatialVelocity();

      EXPECT_TRUE( equals(v_total[i], v_actual) );
    }
  }

  // Testing conversion beteween spatial and classical velocities
  {
    std::vector<Eigen::Vector3d> v_rels(frames.size());
    std::vector<Eigen::Vector3d> w_rels(frames.size());

    std::vector<Eigen::Vector3d> v_total(frames.size());
    std::vector<Eigen::Vector3d> w_total(frames.size());

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<3>();
      w_rels[i] = random_vec<3>();

      SimpleFrame* F = frames[i];
      F->setClassicDerivatives(v_rels[i], w_rels[i]);

      Eigen::Vector3d offset = F->getRelativeTransform().translation();

      Eigen::Isometry3d tf = i>0? frames[i-1]->getTransform() :
          Eigen::Isometry3d::Identity();

      if(i>0)
        compute_velocity(v_total[i-1], w_total[i-1], v_rels[i], w_rels[i],
            offset, tf, v_total[i], w_total[i]);
      else
        compute_velocity(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         v_rels[i], w_rels[i],
                         offset, tf, v_total[i], w_total[i]);
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];
      Eigen::Vector3d v_actual = F->getLinearVelocity();
      Eigen::Vector3d w_actual = F->getAngularVelocity();

      EXPECT_TRUE( equals(v_total[i], v_actual, tolerance) );
      EXPECT_TRUE( equals(w_total[i], w_actual, tolerance) );
    }
  }

  // -- Acceleration ---------------------------------------------------------

  // Basic forward spatial acceleration propagation
  {
    Eigen::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
    Eigen::aligned_vector<Eigen::Vector6d> a_rels(frames.size());

    Eigen::aligned_vector<Eigen::Vector6d> v_total(frames.size());
    Eigen::aligned_vector<Eigen::Vector6d> a_total(frames.size());


    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<6>();
      a_rels[i] = random_vec<6>();

      SimpleFrame* F = frames[i];
      F->setRelativeSpatialVelocity(v_rels[i]);
      F->setRelativeSpatialAcceleration(a_rels[i]);
      Eigen::Isometry3d tf = F->getRelativeTransform();

      if(i>0)
      {
        compute_spatial_velocity(v_total[i-1], v_rels[i], tf, v_total[i]);
        compute_spatial_acceleration(a_total[i-1], a_rels[i], v_total[i],
            v_rels[i], tf, a_total[i]);
      }
      else
      {
        compute_spatial_velocity(Eigen::Vector6d::Zero(), v_rels[i],
                                 tf, v_total[i]);
        compute_spatial_acceleration(Eigen::Vector6d::Zero(), a_rels[i],
                                     v_total[i], v_rels[i], tf, a_total[i]);
      }
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];

      Eigen::Vector6d a_actual = F->getSpatialAcceleration();

      EXPECT_TRUE( equals(a_total[i], a_actual) );
    }

    // Test relative computations
    for(std::size_t i=0; i<frames.size(); ++i)
    {
      Frame* F = frames[i];
      Frame* P = F->getParentFrame();

      Eigen::Vector6d v_rel = F->getSpatialVelocity(P, F);
      Eigen::Vector6d a_rel = F->getSpatialAcceleration(P, F);

      EXPECT_TRUE( equals(v_rels[i], v_rel, tolerance) );
      EXPECT_TRUE( equals(a_rels[i], a_rel, tolerance) );
    }

    // Test offset computations
    for(std::size_t i=0; i<frames.size(); ++i)
    {
      Eigen::Vector3d offset = random_vec<3>();
      Frame* F = frames[i];

      Eigen::Vector3d v_actual = F->getLinearVelocity(offset);
      Eigen::Vector3d w_actual = F->getAngularVelocity();
      Eigen::Vector3d v_expect, w_expect;
      compute_velocity(F->getLinearVelocity(), F->getAngularVelocity(),
                       Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                       offset, F->getWorldTransform(), v_expect, w_expect);

      EXPECT_TRUE( equals( v_expect, v_actual) );
      EXPECT_TRUE( equals( w_expect, w_actual) );

      Eigen::Vector3d a_actual = F->getLinearAcceleration(offset);
      Eigen::Vector3d alpha_actual = F->getAngularAcceleration();
      Eigen::Vector3d a_expect, alpha_expect;
      compute_acceleration(F->getLinearAcceleration(),
                           F->getAngularAcceleration(), F->getAngularVelocity(),
                           Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                           Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                           offset, F->getWorldTransform(),
                           a_expect, alpha_expect);

      EXPECT_TRUE( equals( a_expect, a_actual) );
      EXPECT_TRUE( equals( alpha_expect, alpha_actual) );
    }
  }

  // Testing conversion between spatial and classical accelerations
  {
    std::vector<Eigen::Vector3d> v_rels(frames.size());
    std::vector<Eigen::Vector3d> w_rels(frames.size());
    std::vector<Eigen::Vector3d> a_rels(frames.size());
    std::vector<Eigen::Vector3d> alpha_rels(frames.size());

    std::vector<Eigen::Vector3d> v_total(frames.size());
    std::vector<Eigen::Vector3d> w_total(frames.size());
    std::vector<Eigen::Vector3d> a_total(frames.size());
    std::vector<Eigen::Vector3d> alpha_total(frames.size());

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<3>();
      w_rels[i] = random_vec<3>();
      a_rels[i] = random_vec<3>();
      alpha_rels[i] = random_vec<3>();

      SimpleFrame* F = frames[i];
      Eigen::Isometry3d rel_tf;
      randomize_transform(rel_tf);
      F->setRelativeTransform(rel_tf);
      F->setClassicDerivatives(v_rels[i], w_rels[i], a_rels[i], alpha_rels[i]);

      Eigen::Vector3d offset = F->getRelativeTransform().translation();

      Eigen::Isometry3d tf = i>0? frames[i-1]->getTransform() :
          Eigen::Isometry3d::Identity();

      if(i>0)
      {
        compute_velocity(v_total[i-1], w_total[i-1], v_rels[i], w_rels[i],
            offset, tf, v_total[i], w_total[i]);
        compute_acceleration(a_total[i-1], alpha_total[i-1], w_total[i-1],
            a_rels[i], alpha_rels[i], v_rels[i], w_rels[i], offset, tf,
            a_total[i], alpha_total[i]);
      }
      else
      {
        compute_velocity(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         v_rels[i], w_rels[i], offset, tf,
                         v_total[i], w_total[i]);
        compute_acceleration(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                             Eigen::Vector3d::Zero(), a_rels[i], alpha_rels[i],
                             v_rels[i], w_rels[i], offset, tf,
                             a_total[i], alpha_total[i]);
      }
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];
      Eigen::Vector3d v_actual = F->getLinearVelocity();
      Eigen::Vector3d w_actual = F->getAngularVelocity();
      Eigen::Vector3d a_actual = F->getLinearAcceleration();
      Eigen::Vector3d alpha_actual = F->getAngularAcceleration();

      EXPECT_TRUE( equals(v_total[i], v_actual, tolerance) );
      EXPECT_TRUE( equals(w_total[i], w_actual, tolerance) );
      EXPECT_TRUE( equals(a_total[i], a_actual, tolerance) );
      EXPECT_TRUE( equals(alpha_total[i], alpha_actual, tolerance) );
    }

    // Test relative computations
    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];
      Frame* P = F->getParentFrame();
      Eigen::Vector3d v_rel = F->getLinearVelocity(P, P);
      Eigen::Vector3d w_rel = F->getAngularVelocity(P, P);
      Eigen::Vector3d a_rel = F->getLinearAcceleration(P, P);
      Eigen::Vector3d alpha_rel = F->getAngularAcceleration(P, P);

      EXPECT_TRUE( equals(v_rels[i], v_rel, tolerance) );
      EXPECT_TRUE( equals(w_rels[i], w_rel, tolerance) );
      EXPECT_TRUE( equals(a_rels[i], a_rel, tolerance) );
      EXPECT_TRUE( equals(alpha_rels[i], alpha_rel, tolerance) );
    }
  }
}
Пример #29
0
  void ViewerState::optimizeSelected(){
      DrawableFrame* current = drawableFrameVector.back();
      DrawableFrame* reference = drawableFrameVector[drawableFrameVector.size()-2];
      cerr << "optimizing" << endl;
      cerr << "current=" << current->frame() << endl;
      cerr << "reference= " << reference->frame() << endl;


      

      // cerr computing initial guess based on the frame positions, just for convenience
      Eigen::Isometry3d delta = reference->_vertex->estimate().inverse()*current->_vertex->estimate();
      for(int c=0; c<4; c++)
	for(int r=0; r<3; r++)
	  initialGuess.matrix()(r,c) = delta.matrix()(r,c);


      Eigen::Isometry3f odometryMean;
      Matrix6f odometryInfo;
      bool hasOdometry = extractRelativePrior(odometryMean, odometryInfo, reference, current);
      if (hasOdometry)
	initialGuess=odometryMean;

      Eigen::Isometry3f imuMean;
      Matrix6f imuInfo;
      bool hasImu = extractAbsolutePrior(imuMean, imuInfo, current);

      initialGuess.matrix().row(3) << 0,0,0,1;
      
      if(!wasInitialGuess) {
	aligner->clearPriors();
	aligner->setOuterIterations(al_outerIterations);
	aligner->setReferenceFrame(reference->frame());
	aligner->setCurrentFrame(current->frame());
	aligner->setInitialGuess(initialGuess);
	aligner->setSensorOffset(sensorOffset);
	if (hasOdometry)
	  aligner->addRelativePrior(odometryMean, odometryInfo);
	if (hasImu)
	  aligner->addAbsolutePrior(reference->transformation(), imuMean, imuInfo);
	aligner->align();
      }

      Eigen::Isometry3f localTransformation =aligner->T();
      if (aligner->inliers()<1000 || aligner->error()/aligner->inliers()>10){
	cerr << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
	cerr << "aligner: monster failure: inliers = " << aligner->inliers() << endl;
	cerr << "aligner: monster failure: error/inliers = " << aligner->error()/aligner->inliers() << endl;
	cerr  << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
      	localTransformation = initialGuess;
	sleep(1);
      }
      cout << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
      
      globalT = reference->transformation()*aligner->T();
      
      // Update cloud drawing position.
      current->setTransformation(globalT);
      current->setLocalTransformation(aligner->T());

      // Show zBuffers.
      refScn->clear();
      currScn->clear();
      QImage refQImage;
      QImage currQImage;
      DepthImageView div;
      div.computeColorMap(300, 2000, 128);
      div.convertToQImage(refQImage, aligner->correspondenceFinder()->referenceDepthImage());
      div.convertToQImage(currQImage, aligner->correspondenceFinder()->currentDepthImage());
      refScn->addPixmap((QPixmap::fromImage(refQImage)).scaled(QSize((int)refQImage.width()/(ng_scale*3), (int)(refQImage.height()/(ng_scale*3)))));
      currScn->addPixmap((QPixmap::fromImage(currQImage)).scaled(QSize((int)currQImage.width()/(ng_scale*3), (int)(currQImage.height()/(ng_scale*3)))));
      pwnGMW->graphicsView1_2d->show();
      pwnGMW->graphicsView2_2d->show();
      
      wasInitialGuess = false;
      newCloudAdded = false;
      *initialGuessViewer = 0;
      *optimizeViewer = 0;
  }
Пример #30
0
void BundleAdjustmentOptimization(const vector<Point3f>& objectPoints,const vector<Eigen::Vector2d>& imagePoints1,const vector<Eigen::Vector2d>& imagePoints2)
{

    typedef BlockSolver_6_3 SlamBlockSolver;
    typedef LinearSolverEigen< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;

//    Eigen::Quaternionf r = Eigen::Quaterniond(T.rotation());
//    Eigen::Quaterniond rr(r.w(),r.x(),r.y(),r.z());
//    Eigen::Vector3f t = T.translation();
//    Eigen::Vector3d tt(t[0],t[1],t[2]);

//    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
//    //SlamLinearSolver* linearSolver = new SlamLinearSolver();
//    SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
//    OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(blockSolver);

//    //OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(blockSolver);

//    //Eigen::Vector2d principal_point(325.5,253.5);
//    Eigen::Vector2d principal_point(320,240);
//    //CameraParameters * cam_params = new CameraParameters (518.5, principal_point,0);
//    CameraParameters * cam_params = new CameraParameters (518.5, principal_point,0);
//    cam_params->setId(0);
//    SparseOptimizer optimizer;
//    optimizer.setAlgorithm(solver);
//    optimizer.setVerbose(false);
//    optimizer.addParameter(cam_params);



//    int id=0;

//    for(int i =0;i<2;i++)
//    {

//        VertexSE3Expmap* se3 = new VertexSE3Expmap();
//        se3->setId(id);
//        if(i == 0)
//        {
//            se3->setFixed(true);
//            se3->setEstimate(SE3Quat());
//        }
//        else
//        {  //SE3Quat* pos = new SE3Quat(rr,tt);
//            se3->setEstimate(SE3Quat());
//        }
//        optimizer.addVertex(se3);
//        id++;
//    }

//    cout<<"obejecPoints.size = ";
//    cout<<objectPoints.size()<<endl;
//    for(int i =0;i<objectPoints.size();i++)
//    {
//        VertexSBAPointXYZ* v_xyz = new VertexSBAPointXYZ();
//        Eigen::Vector3d vec;
//        v_xyz->setId(id);
//        vec<<objectPoints[i].x,objectPoints[i].y,objectPoints[i].z;
//        v_xyz->setEstimate(vec);
//        v_xyz->setMarginalized(false);
//        optimizer.addVertex(v_xyz);
//        id++;

//    }


//    cout<<imagePoints1.size()<<endl;
//    for(int i =0;i<imagePoints1.size();i++)
//    {
//        EdgeProjectXYZ2UV* e = new EdgeProjectXYZ2UV();
//        e->setVertex(0, dynamic_cast<VertexSBAPointXYZ*> (optimizer.vertex(2+i)));
//        e->setVertex(1, dynamic_cast<VertexSE3Expmap*> (optimizer.vertex(0)));
//        e->setMeasurement(imagePoints1[i]);
//        e->information() = Eigen::Matrix2d::Identity();
//        e->setParameterId(0,0);
//        RobustKernelHuber* rk = new RobustKernelHuber;
//        e->setRobustKernel(rk);
//        optimizer.addEdge(e);
//    }


//    cout<<imagePoints2.size()<<endl;
//    for(int i =0;i<imagePoints2.size();i++)
//    {
//        EdgeProjectXYZ2UV* e = new EdgeProjectXYZ2UV();
//        e->setVertex(0, dynamic_cast<VertexSBAPointXYZ*> (optimizer.vertex(2+i)));
//        e->setVertex(1, dynamic_cast<VertexSE3Expmap*> (optimizer.vertex(1)));
//        e->setMeasurement(imagePoints2[i]);
//        e->information() = Eigen::Matrix2d::Identity();
//        RobustKernelHuber* rk = new RobustKernelHuber;
//        e->setParameterId(0,0);
//        e->setRobustKernel(rk);
//        optimizer.addEdge(e);
//    }

    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降
    //g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );
    OptimizationAlgorithmDogleg* algorithm = new OptimizationAlgorithmDogleg(block_solver);
    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );


    g2o::CameraParameters* camera = new g2o::CameraParameters( 518.5, Eigen::Vector2d(325,253), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );
    // 添加节点
    // 两个位姿节点
    for ( int i=0; i<2; i++ )
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose,因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<objectPoints.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道,只能把深度设置为1了

        Eigen::Vector3d vec;
        vec<<objectPoints[i].x,objectPoints[i].y,objectPoints[i].z;
        v->setMarginalized(true);
        v->setEstimate( vec );
        optimizer.addVertex( v );
    }


    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<imagePoints1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( imagePoints1[i] );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<imagePoints2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( imagePoints2[i] );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    int inliers=0;
    for ( vector<g2o::EdgeProjectXYZ2UV*>::iterator e = edges.begin();e!=edges.end();e++ )
    {
        (*e)->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大,说明此边的值与其他边很不相符
        if ( (*e)->chi2() > 1 )
        {
            cout<<"error = "<<(*e)->chi2()<<endl;
        }
        else
        {
            inliers++;
        }
    }


    optimizer.setVerbose(true);
    optimizer.initializeOptimization();

    cout<<"optimizer start"<<endl;
    optimizer.optimize(1,true);
    cout<<"optimizer end"<<endl;

    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;


    for ( size_t i=0; i<imagePoints1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }
}