コード例 #1
0
ファイル: state.cpp プロジェクト: GCTMODS/nmpc
/*
Runs the kinematics model on the state vector and returns a vector with the
derivative of each components (except for the accelerations, which must be
calculated directly using a dynamics model).
Contents are as follows:
    - Rate of change in position (3-vector, m/s, NED frame)
    - Rate of change in linear velocity (3-vector, m/s^2, NED frame)
    - Rate of change in attitude (quaternion (x, y, z, w), 1/s, body frame)
    - Rate of change in angular velocity (3-vector, rad/s^2, body frame)
*/
const StateVectorDerivative State::model(ControlVector c, DynamicsModel *d) {
    StateVectorDerivative output;

    AccelerationVector a = d->evaluate(*this, c);

    /* Calculate change in position. */
    output.segment<3>(0) << velocity();

    /* Calculate change in velocity. */
    Quaternionr attitude_q = Quaternionr(attitude());
    output.segment<3>(3) = attitude_q.conjugate() * a.segment<3>(0);

    /* Calculate change in attitude. */
    Eigen::Matrix<real_t, 4, 1> omega_q;
    omega_q << angular_velocity(), 0;

    attitude_q = Quaternionr(omega_q).conjugate() * attitude_q;
    output.segment<4>(6) << attitude_q.vec(), attitude_q.w();
    output.segment<4>(6) *= 0.5;

    /* Calculate change in angular velocity (just angular acceleration). */
    output.segment<3>(10) = a.segment<3>(3);

    return output;
}
コード例 #2
0
ファイル: gyro_lib.cpp プロジェクト: srmx/AltIMU-10
bool device_gyro::calibrate()
{
    static const uint_t n = 512;
    std::vector<vec3d> meas(n);

    // measure
    for (uint_t i = 0; i < n; ++ i)
    {
        meas[i] = angular_velocity();
        sysclock.delay(1);
    }

    // mean value
    vec3d mean;

    for (uint_t i = 0; i < n; ++ i)
        mean = mean + meas[i];

    mean = mean / n;

    // standard deviation
    float dev = 0;

    for (uint_t i = 0; i < n; ++ i)
        dev += (meas[i] - mean).square();

    dev = sqrt(dev / n);

    if (dev > 0.05)
        return false;

    w0 = w0 + mean;
    return true;
}
コード例 #3
0
ファイル: LiveSLAMWrapper.cpp プロジェクト: kanster/dense_new
void LiveSLAMWrapper::BALoop()
{
    ros::Rate BARate(2000) ;
    list<ImageMeasurement>::iterator iterImage ;
    std::list<visensor_node::visensor_imu>::iterator iterIMU ;
    cv::Mat image0 ;
    cv::Mat image1 ;
    cv::Mat gradientMapForDebug(height, width, CV_8UC3) ;
    sensor_msgs::Image msg;
    double t ;

    while ( nh.ok() )
    {
        monoOdometry->frameInfoList_mtx.lock();
        int ttt = (monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead);
        if ( ttt < 0 ){
            ttt += frameInfoListSize ;
        }
        //printf("[BA thread] sz=%d\n", ttt ) ;
        if ( ttt < 1 ){
            monoOdometry->frameInfoList_mtx.unlock();
            BARate.sleep() ;
            continue ;
        }
        for ( int sz ; ; )
        {
            monoOdometry->frameInfoListHead++ ;
            if ( monoOdometry->frameInfoListHead >= frameInfoListSize ){
                monoOdometry->frameInfoListHead -= frameInfoListSize ;
            }
            sz = monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead ;
            if ( sz == 0 ){
                break ;
            }
            if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ){
                break ;
            }
        }
        ros::Time imageTimeStamp = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].t ;
        monoOdometry->frameInfoList_mtx.unlock();

        //Pop out the image list
        image1_queue_mtx.lock();
        iterImage = image1Buf.begin() ;
        while ( iterImage->t < imageTimeStamp ){
            iterImage = image1Buf.erase( iterImage ) ;
        }
        image1 = iterImage->image.clone();
        image1_queue_mtx.unlock();

        image0_queue_mtx.lock();
        iterImage = image0Buf.begin() ;
        while ( iterImage->t < imageTimeStamp ){
            iterImage = image0Buf.erase( iterImage ) ;
        }
        image0 = iterImage->image.clone();
        image0_queue_mtx.unlock();

        imu_queue_mtx.lock();
        iterIMU = imuQueue.begin() ;
        Vector3d linear_acceleration;
        Vector3d angular_velocity;

        //std::cout << "imageTime=" << imageTimeStamp << std::endl;
        while ( iterIMU->header.stamp < imageTimeStamp )
        {
            linear_acceleration(0) = iterIMU->linear_acceleration.x;
            linear_acceleration(1) = iterIMU->linear_acceleration.y;
            linear_acceleration(2) = iterIMU->linear_acceleration.z;
            angular_velocity(0) = iterIMU->angular_velocity.x;
            angular_velocity(1) = iterIMU->angular_velocity.y;
            angular_velocity(2) = iterIMU->angular_velocity.z;

//            to_pub_info.x = angular_velocity(0)*180/PI ;
//            to_pub_info.y = angular_velocity(1)*180/PI ;
//            to_pub_info.z = angular_velocity(2)*180/PI ;
//            monoOdometry->pub_angular_velocity.publish( to_pub_info ) ;

//            outFile << to_pub_info.x << " "
//                    << to_pub_info.y << " "
//                    << to_pub_info.z << "\n";


            double pre_t = iterIMU->header.stamp.toSec();
            iterIMU = imuQueue.erase(iterIMU);
            double next_t = iterIMU->header.stamp.toSec();
            double dt = next_t - pre_t ;

//            std::cout << linear_acceleration.transpose() << std::endl ;
//            std::cout << angular_velocity.transpose() << std::endl ;
            monoOdometry->processIMU( dt, linear_acceleration, angular_velocity );
        }
        imu_queue_mtx.unlock();

        //propagate the last frame info to the current frame
        Frame* lastFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
        float dt = lastFrame->timeIntegral;

        Vector3d T_bk1_2_b0 = lastFrame->T_bk_2_b0 - 0.5 * gravity_b0 * dt *dt
                + lastFrame->R_bk_2_b0*(lastFrame->v_bk * dt  + lastFrame->alpha_c_k);
        Vector3d v_bk1 = lastFrame->R_k1_k.transpose() *
                (lastFrame->v_bk - lastFrame->R_bk_2_b0.transpose() * gravity_b0 * dt
                 + lastFrame->beta_c_k);
        Matrix3d R_bk1_2_b0 = lastFrame->R_bk_2_b0 * lastFrame->R_k1_k;

        monoOdometry->insertFrame(imageSeqNumber, image0, imageTimeStamp, R_bk1_2_b0, T_bk1_2_b0, v_bk1);
        Frame* currentFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
        Frame* keyFrame = monoOdometry->currentKeyFrame.get();
        if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag )
        {
            //prepare key frame
            cv::Mat disparity, depth ;
            monoOdometry->bm_(image0, image1, disparity, CV_32F);
            calculateDepthImage(disparity, depth, 0.11, fx );
            currentFrame->setDepthFromGroundTruth( (float*)depth.data ) ;

#ifdef PRINT_DEBUG_INFO
            //pub debugMap
            cv::cvtColor(image0, gradientMapForDebug, CV_GRAY2BGR);
            monoOdometry->generateDubugMap(currentFrame, gradientMapForDebug);
            msg.header.stamp = imageTimeStamp;
            sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::BGR8, height,
                                   width, width*3, gradientMapForDebug.data );
            monoOdometry->pub_gradientMapForDebug.publish(msg) ;
#endif
            int preKeyFrameID = monoOdometry->currentKeyFrame->id() ;

            //set key frame
            monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[monoOdometry->tail] ;
            monoOdometry->currentKeyFrame->keyFrameFlag = true ;
            monoOdometry->currentKeyFrame->cameraLinkList.clear() ;

            //reset the initial guess
            monoOdometry->RefToFrame = Sophus::SE3() ;

            //update tracking reference
            monoOdometry->updateTrackingReference();

#ifdef PUB_POINT_CLOUD
            pubPointCloud(valid_num, imageTimeStamp, R_vi_2_odometry);
#endif
            //unlock dense tracking
            monoOdometry->tracking_mtx.lock();
            monoOdometry->lock_densetracking = false;
            monoOdometry->tracking_mtx.unlock();

            if ( (imageTimeStamp - lastLoopClorsureTime).toSec() > 0.18 )
            {
                //add possible loop closure link
                t = (double)cvGetTickCount()  ;
                monoOdometry->setReprojectionListRelateToLastestKeyFrame( monoOdometry->head, preKeyFrameID,
                                                                          monoOdometry->slidingWindow[monoOdometry->tail].get(),
                                                                          R_i_2_c, T_i_2_c ) ;
                ROS_WARN("loop closure link cost time: %f", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
                t = (double)cvGetTickCount()  ;
                lastLoopClorsureTime = imageTimeStamp ;
            }
        }

        if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust )
        {
//            cout << "insert camera link" << endl ;
//            cout << monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c << endl ;

            monoOdometry->insertCameraLink(keyFrame, currentFrame,
                          monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].R_k_2_c,
                          monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c,
                          monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].lastestATA );
        }

        int control_flag = 0 ;
        Vector3d preBAt = currentFrame->T_bk_2_b0 ;


//        cout << "[-BA]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
//        cout << "[-BA]current Velocity: " << currentFrame->v_bk.transpose() << endl;

        //BA
        t = (double)cvGetTickCount()  ;
        monoOdometry->BA();
        printf("BA cost time: %f\n", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
        t = (double)cvGetTickCount()  ;

        //cout << "[BA-]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
        //cout << "[BA-]current Velocity: " << currentFrame->v_bk.transpose() << endl;

        if ( (currentFrame->T_bk_2_b0 - preBAt ).norm() > 0.1 ){
            control_flag = 1 ; //loop_closure or other sudden position change case
        }
        if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust == false ){
            control_flag = 2 ; //only IMU link, dense tracking fails
        }

#ifdef PUB_GRAPH
        pubCameraLink();
#endif

        //marginalziation
        monoOdometry->twoWayMarginalize();
        monoOdometry->setNewMarginalzationFlag();

//        R_vi_2_odometry.setIdentity() ;
//        if ( onUAV )
//        {
//            pubOdometry(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
//                    monoOdometry->slidingWindow[monoOdometry->tail]->v_bk,
//                    monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
//                    monoOdometry->pub_odometry, monoOdometry->pub_pose,
//                    control_flag, R_vi_2_odometry,
//                    monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag, imageTimeStamp );
//        }
//        else
//        {
//            pubOdometry(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
//                    monoOdometry->slidingWindow[monoOdometry->tail]->v_bk,
//                    monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
//                    monoOdometry->pub_odometry, monoOdometry->pub_pose,
//                    control_flag, Eigen::Matrix3d::Identity(),
//                    monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag, imageTimeStamp );
//        }

#ifdef PRINT_DEBUG_INFO
        int colorFlag = 0 ;
        colorFlag = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ;
        if (  monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust == false ){
            colorFlag = 2 ;
        }
        if ( onUAV ){
            pubPath(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
                    colorFlag,
                    monoOdometry->path_line, monoOdometry->pub_path, R_vi_2_odometry);
        }
        else{
            pubPath(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
                    colorFlag,
                    monoOdometry->path_line, monoOdometry->pub_path, Eigen::Matrix3d::Identity() );
        }
#endif

#ifdef  PUB_TF
        static tf::TransformBroadcaster br;
        tf::Transform transform;
        Vector3d t_translation = R_vi_2_odometry * monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0 ;
        Quaterniond t_q(monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0) ;
        transform.setOrigin(tf::Vector3(t_translation(0),
                                t_translation(1),
                                t_translation(2)) );
        tf::Quaternion q;
        Quaterniond tt_q(R_vi_2_odometry * monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0 * R_vi_2_odometry.transpose());
        q.setW(tt_q.w());
        q.setX(tt_q.x());
        q.setY(tt_q.y());
        q.setZ(tt_q.z());
        transform.setRotation(q);
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "densebody"));
#endif
        logCameraPose() ;

//        int preIndex = monoOdometry->tail - 1 ;
//        if ( preIndex < 0 ){
//            preIndex += slidingWindowSize ;
//        }
//        Vector3d tt_dist = (monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0 -
//                monoOdometry->slidingWindow[preIndex]->T_bk_2_b0) ;
//        Matrix3d tt_rotate = monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0.transpose() *
//                monoOdometry->slidingWindow[preIndex]->R_bk_2_b0 ;
//        Quaterniond tt_q(tt_rotate) ;

//        to_pub_info.x = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(0) ;
//        to_pub_info.y = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(1) ;
//        to_pub_info.z = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(2) ;
//        monoOdometry->pub_linear_velocity.publish(to_pub_info) ;


    }
}
コード例 #4
0
ファイル: body_description.cpp プロジェクト: Cassie90/ClanLib
BodyDescription::BodyDescription(const PhysicsContext &pc, const std::string &resource_id, const XMLResourceDocument &resources)
: impl(new BodyDescription_Impl(pc.impl->get_owner()))
{
	/* example resource entry with all parameters:
	
	<body2d name="TestBody" type="static">
        <position x="0" y="0"/>
        <rotation angle="180"/>
        <velocity x="0" y="0" angular="0"/>
        <damping linear="0" angular="0"/>
		<parameters awake="true" can_sleep="true" bullet="false" active="true" />
    </body2d>

	*/
	XMLResourceNode resource = resources.get_resource(resource_id);
	if (resource.get_type() != "body2d" && resource.get_type() != "body2d_description")
		throw Exception(string_format("Resource '%1' is not of type 'body2d' or 'body2d_description'", resource_id));

	DomNode cur_node = resource.get_element().get_first_child();

	//Body type
	std::string body_type = resource.get_element().get_attribute("type","static");
	if(body_type == "kinematic") set_type(body_kinematic);
	else if(body_type == "dynamic") set_type(body_dynamic);
	else set_type(body_static);

	while(!cur_node.is_null())
	{
		if (!cur_node.is_element()) 
			continue;
		
		DomElement cur_element = cur_node.to_element();
		std::string tag_name = cur_element.get_tag_name();

		//<position x="0" y="0"/>
		if(tag_name == "position")
		{
			float pos_x = 0.0f;
			float pos_y = 0.0f;

			if(cur_element.has_attribute("x"))
			{
				pos_x = StringHelp::text_to_float(cur_element.get_attribute("x"));
			}

			if(cur_element.has_attribute("y"))
			{
				pos_y = StringHelp::text_to_float(cur_element.get_attribute("y"));
			}

			set_position(pos_x, pos_y);
		}

		//<rotation angle="180"/>
		else if(tag_name == "rotation")
		{
			Angle angle(0.0f, angle_degrees);

			if(cur_element.has_attribute("angle"))
			{
				angle = Angle(StringHelp::text_to_float(cur_element.get_attribute("angle")), angle_degrees);
			}

			set_angle(angle);
		}

		//<velocity x="0" y="0" angular="0"/>
		else if(tag_name == "velocity")
		{
			Vec2f velocity(0.0f, 0.0f);
			Angle angular_velocity(0.0f, angle_degrees);

			if(cur_element.has_attribute("x"))
			{
				velocity.x = StringHelp::text_to_float(cur_element.get_attribute("x"));
			}

			if(cur_element.has_attribute("y"))
			{
				velocity.y = StringHelp::text_to_float(cur_element.get_attribute("y"));
			}

			if(cur_element.has_attribute("angular"))
			{
				angular_velocity = Angle(StringHelp::text_to_float(cur_element.get_attribute("angular")), angle_degrees);
			}

			set_linear_velocity(velocity);
			set_angular_velocity(angular_velocity);
		}

		//<damping linear="0" angular="0"/>
		else if(tag_name == "damping")
		{
			float linear;
			float angular;

			if(cur_element.has_attribute("linear"))
			{
				linear = StringHelp::text_to_float(cur_element.get_attribute("linear"));
			}

			if(cur_element.has_attribute("angular"))
			{
				angular = StringHelp::text_to_float(cur_element.get_attribute("angular"));
			}

			set_linear_damping(linear);
			set_angular_damping(angular);
		}

		//<parameters awake="true" can_sleep="true" bullet="false" active="true" />
		else if(tag_name == "parameters")
		{
			bool value;
		
			if(cur_element.has_attribute("awake"))
			{
				value = true;
				value = StringHelp::text_to_bool(cur_element.get_attribute("awake"));
				set_awake(value);
			}
			if(cur_element.has_attribute("active"))
			{
				value = true;
				value = StringHelp::text_to_bool(cur_element.get_attribute("active"));
				set_active(value);
			}
			if(cur_element.has_attribute("bullet"))
			{
				value = false;
				value = StringHelp::text_to_bool(cur_element.get_attribute("bullet"));
				set_as_bullet(value);
			}
			if(cur_element.has_attribute("can_sleep"))
			{
				value = true;
				value = StringHelp::text_to_bool(cur_element.get_attribute("can_sleep"));
				allow_sleep(value);
			}
		}

		cur_node = cur_node.get_next_sibling();
	}
}
コード例 #5
0
void LiveSLAMWrapper::BALoop()
{
    ros::Rate BARate(2000) ;
    list<ImageMeasurement>::iterator iterImage ;
    std::list<visensor_node::visensor_imu>::iterator iterIMU ;
    cv::Mat image0 ;
    cv::Mat image1 ;
    cv::Mat gradientMapForDebug(height, width, CV_8UC3) ;
    sensor_msgs::Image msg;
    double t ;

    while ( nh.ok() )
    {
        monoOdometry->frameInfoList_mtx.lock();
        int ttt = (monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead);
        if ( ttt < 0 ){
            ttt += frameInfoListSize ;
        }
        //printf("[BA thread] sz=%d\n", ttt ) ;
        if ( ttt < 1 ){
            monoOdometry->frameInfoList_mtx.unlock();
            BARate.sleep() ;
            continue ;
        }
        for ( int sz ; ; )
        {
            monoOdometry->frameInfoListHead++ ;
            if ( monoOdometry->frameInfoListHead >= frameInfoListSize ){
                monoOdometry->frameInfoListHead -= frameInfoListSize ;
            }
            sz = monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead ;
            if ( sz == 0 ){
                break ;
            }
            if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ){
                break ;
            }
        }
        ros::Time imageTimeStamp = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].t ;
        monoOdometry->frameInfoList_mtx.unlock();

        //Pop out the image list
        image1_queue_mtx.lock();
        iterImage = image1Buf.begin() ;
        while ( iterImage->t < imageTimeStamp ){
            iterImage = image1Buf.erase( iterImage ) ;
        }
        image1 = iterImage->image.clone();
        image1_queue_mtx.unlock();

        image0_queue_mtx.lock();
        iterImage = image0Buf.begin() ;
        while ( iterImage->t < imageTimeStamp ){
            iterImage = image0Buf.erase( iterImage ) ;
        }
        image0 = iterImage->image.clone();
        image0_queue_mtx.unlock();

        imu_queue_mtx.lock();
        iterIMU = imuQueue.begin() ;
        Vector3d linear_acceleration;
        Vector3d angular_velocity;

        //std::cout << "imageTime=" << imageTimeStamp << std::endl;
        while ( iterIMU->header.stamp < imageTimeStamp )
        {
            linear_acceleration(0) = iterIMU->linear_acceleration.x;
            linear_acceleration(1) = iterIMU->linear_acceleration.y;
            linear_acceleration(2) = iterIMU->linear_acceleration.z;
            angular_velocity(0) = iterIMU->angular_velocity.x;
            angular_velocity(1) = iterIMU->angular_velocity.y;
            angular_velocity(2) = iterIMU->angular_velocity.z;

            //linear_acceleration = -linear_acceleration;
            //angular_velocity = -angular_velocity ;

//            double pre_t = iterIMU->header.stamp.toSec();
//            iterIMU = imuQueue.erase(iterIMU);

//            //std::cout << imuQueue.size() <<" "<< iterIMU->header.stamp << std::endl;

//            double next_t = iterIMU->header.stamp.toSec();
//            double dt = next_t - pre_t ;

            double pre_t = iterIMU->header.stamp.toSec();
            iterIMU = imuQueue.erase(iterIMU);
            double next_t = iterIMU->header.stamp.toSec();
            double dt = next_t - pre_t ;

//            std::cout << linear_acceleration.transpose() << std::endl ;
//            std::cout << angular_velocity.transpose() << std::endl ;
            monoOdometry->processIMU( dt, linear_acceleration, angular_velocity );
        }
        imu_queue_mtx.unlock();

        //propagate the last frame info to the current frame
        Frame* lastFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
        float dt = lastFrame->timeIntegral;

        Vector3d T_bk1_2_b0 = lastFrame->T_bk_2_b0 - 0.5 * gravity_b0 * dt *dt
                + lastFrame->R_bk_2_b0*(lastFrame->v_bk * dt  + lastFrame->alpha_c_k);
        Vector3d v_bk1 = lastFrame->R_k1_k.transpose() *
                (lastFrame->v_bk - lastFrame->R_bk_2_b0.transpose() * gravity_b0 * dt
                 + lastFrame->beta_c_k);
        Matrix3d R_bk1_2_b0 = lastFrame->R_bk_2_b0 * lastFrame->R_k1_k;

        monoOdometry->insertFrame(imageSeqNumber, image1, imageTimeStamp, R_bk1_2_b0, T_bk1_2_b0, v_bk1);
        Frame* currentFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
        Frame* keyFrame = monoOdometry->currentKeyFrame.get();
        if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag )
        {
            //prepare key frame
            cv::Mat disparity, depth ;
            monoOdometry->bm_(image1, image0, disparity, CV_32F);
            calculateDepthImage(disparity, depth, 0.11, fx );
            currentFrame->setDepthFromGroundTruth( (float*)depth.data ) ;

            //pub debugMap
            cv::cvtColor(image1, gradientMapForDebug, CV_GRAY2BGR);
            monoOdometry->generateDubugMap(currentFrame, gradientMapForDebug);
            msg.header.stamp = imageTimeStamp;
            sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::BGR8, height,
                                   width, width*3, gradientMapForDebug.data );
            monoOdometry->pub_gradientMapForDebug.publish(msg) ;

            //set key frame
            monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[monoOdometry->tail] ;
            monoOdometry->currentKeyFrame->keyFrameFlag = true ;
            monoOdometry->currentKeyFrame->cameraLinkList.clear() ;
            //reset the initial guess
            monoOdometry->RefToFrame = Sophus::SE3() ;

            //unlock dense tracking
            monoOdometry->tracking_mtx.lock();
            monoOdometry->lock_densetracking = false;
            monoOdometry->tracking_mtx.unlock();
        }
        if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust )
        {
//            cout << "insert camera link" << endl ;
//            cout << monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c << endl ;

            monoOdometry->insertCameraLink(keyFrame, currentFrame,
                          monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].R_k_2_c,
                          monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c,
                          monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].lastestATA );
        }

        cout << "[-BA]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
        cout << "[-BA]current Velocity: " << currentFrame->v_bk.transpose() << endl;

        //BA
        t = (double)cvGetTickCount()  ;
        monoOdometry->BA();
        printf("BA cost time: %f\n", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
        t = (double)cvGetTickCount()  ;

        cout << "[BA-]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
        cout << "[BA-]current Velocity: " << currentFrame->v_bk.transpose() << endl;

        //marginalziation
        monoOdometry->twoWayMarginalize();
        monoOdometry->setNewMarginalzationFlag();

        //    pubOdometry(-T_bk1_2_b0, R_bk1_2_b0, pub_odometry, pub_pose );
        //    pubPath(-T_bk1_2_b0, path_line, pub_path );

        pubOdometry(-monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
                monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
                monoOdometry->pub_odometry, monoOdometry->pub_pose );
        pubPath(-monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
                monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag,
                monoOdometry->path_line, monoOdometry->pub_path);
    }
}