C++ SE3::translation方法代码示例

void DebugOutput3DWrapper::publishTrackedFrame(Frame* kf)
{
	KeyFrameMessage fMsg;


	fMsg.id = kf->id();
	fMsg.time = kf->timestamp();
	fMsg.isKeyframe = false;


	memcpy(fMsg.camToWorld.data(),kf->getScaledCamToWorld().cast<float>().data(),sizeof(float)*7);
	fMsg.fx = kf->fx(publishLvl);
	fMsg.fy = kf->fy(publishLvl);
	fMsg.cx = kf->cx(publishLvl);
	fMsg.cy = kf->cy(publishLvl);
	fMsg.width = kf->width(publishLvl);
	fMsg.height = kf->height(publishLvl);

	/*fMsg.pointcloud.clear();

	liveframe_publisher.publish(fMsg);*/


	SE3 camToWorld = se3FromSim3(kf->getScaledCamToWorld());

	/*geometry_msgs::PoseStamped pMsg;

	pMsg.pose.position.x = camToWorld.translation()[0];
	pMsg.pose.position.y = camToWorld.translation()[1];
	pMsg.pose.position.z = camToWorld.translation()[2];
	pMsg.pose.orientation.x = camToWorld.so3().unit_quaternion().x();
	pMsg.pose.orientation.y = camToWorld.so3().unit_quaternion().y();
	pMsg.pose.orientation.z = camToWorld.so3().unit_quaternion().z();
	pMsg.pose.orientation.w = camToWorld.so3().unit_quaternion().w();

	if (pMsg.pose.orientation.w < 0)
	{
		pMsg.pose.orientation.x *= -1;
		pMsg.pose.orientation.y *= -1;
		pMsg.pose.orientation.z *= -1;
		pMsg.pose.orientation.w *= -1;
	}

	pMsg.header.stamp = ros::Time(kf->timestamp());
	pMsg.header.frame_id = "world";
	pose_publisher.publish(pMsg);*/

	
	cv::circle(tracker_display, cv::Point(320+camToWorld.translation()[0]*640, -240 + camToWorld.translation()[1]*480), 2, cv::Scalar(255, 0, 0),4);
	cv::imshow("Tracking_output", tracker_display);
	std::cout << "PublishTrackedKeyframe: " << camToWorld.translation()[0] << " " << camToWorld.translation()[1] << "  " << camToWorld.translation()[2] << std::endl;
}

void ROSOutput3DWrapper::publishTrackedFrame(Frame* kf)
{
	lsd_slam_viewer::keyframeMsg fMsg;


	fMsg.id = kf->id();
	fMsg.time = kf->timeStampNs()/1000000.0;
	fMsg.isKeyframe = false;


	memcpy(fMsg.camToWorld.data(),kf->getScaledCamToWorld().cast<float>().data(),sizeof(float)*7);
	fMsg.fx = kf->fx(publishLvl);
	fMsg.fy = kf->fy(publishLvl);
	fMsg.cx = kf->cx(publishLvl);
	fMsg.cy = kf->cy(publishLvl);
	fMsg.width = kf->width(publishLvl);
	fMsg.height = kf->height(publishLvl);

	fMsg.pointcloud.clear();

	liveframe_publisher.publish(fMsg);


	SE3 camToWorld = se3FromSim3(kf->getScaledCamToWorld());

	geometry_msgs::PoseStamped pMsg;

	pMsg.pose.position.x = camToWorld.translation()[0];
	pMsg.pose.position.y = camToWorld.translation()[1];
	pMsg.pose.position.z = camToWorld.translation()[2];
	pMsg.pose.orientation.x = camToWorld.so3().unit_quaternion().x();
	pMsg.pose.orientation.y = camToWorld.so3().unit_quaternion().y();
	pMsg.pose.orientation.z = camToWorld.so3().unit_quaternion().z();
	pMsg.pose.orientation.w = camToWorld.so3().unit_quaternion().w();

	if (pMsg.pose.orientation.w < 0)
	{
		pMsg.pose.orientation.x *= -1;
		pMsg.pose.orientation.y *= -1;
		pMsg.pose.orientation.z *= -1;
		pMsg.pose.orientation.w *= -1;
	}

	pMsg.header.stamp = ros::Time(kf->timeStampNs()/1000000.0);
	pMsg.header.frame_id = "world";
	pose_publisher.publish(pMsg);
}

    Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
    {
        Eigen::Matrix <double,6,3> X_subspace;
        X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
        X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();

        return X_subspace;
    }

 Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
 { 
   /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
   Eigen::Matrix<double,6,1> res;
   res.tail<3>() = m.rotation() * axis;
   res.head<3>() = m.translation().cross(res.tail<3>());
   return res;
 }

      Eigen::Matrix <Scalar_t,6,3> se3Action (const SE3 & m) const
      {
        Eigen::Matrix <double,6,3> X_subspace;
        X_subspace.block <3,2> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ().leftCols <2> ();
        X_subspace.block <3,1> (Motion::LINEAR, 2) = m.rotation ().rightCols <1> ();

        X_subspace.block <3,2> (Motion::ANGULAR, 0) = m.rotation ().leftCols <2> ();
        X_subspace.block <3,1> (Motion::ANGULAR, 2).setZero ();

        return X_subspace;
      }

void Visualizer::visualizeMarkers(
    const FramePtr& frame,
    const set<FramePtr>& core_kfs,
    const Map& map,
    bool inited,
    double svo_scale,
    double our_scale)
{
  if((frame == NULL) || !inited)
  {
      vk::output_helper::publishTfTransform(
//                  SE3(Matrix3d::Identity(), Vector3d::Zero()),
                  T_world_from_vision_,
                  ros::Time(frame->timestamp_), "odom", "cam_pos", br_);
      return;
  }

  SE3 temp = (frame->T_f_w_*T_world_from_vision_.inverse()).inverse();
  double scale = our_scale / svo_scale;
  temp.translation() = temp.translation()* scale;

  vk::output_helper::publishTfTransform(
      temp,
      ros::Time(frame->timestamp_), "odom", "cam_pos", br_);

  if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
  {
    vk::output_helper::publishHexacopterMarker(
        pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_),
        1, 0, 0.3, Vector3d(0.,0.,1.));
    vk::output_helper::publishPointMarker(
        pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
        ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
    if(frame->isKeyframe() || publish_map_every_frame_)
      publishMapRegion(core_kfs);
    removeDeletedPts(map);
  }
}

bool depthFromTriangulation(
    const SE3& T_search_ref,
    const Vector3d& f_ref,
    const Vector3d& f_cur,
    double& depth)
{
  Matrix<double,3,2> A; A << T_search_ref.rotationMatrix() * f_ref, f_cur;
  const Matrix2d AtA = A.transpose()*A;
  if(AtA.determinant() < 0.000001)
    return false;
  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
  depth = fabs(depth2[0]);
  return true;
}

double DepthFilter::computeTau(
      const SE3& T_ref_cur,
      const Vector3d& f,
      const double z,
      const double px_error_angle)
{
  Vector3d t(T_ref_cur.translation());
  Vector3d a = f*z-t;
  double t_norm = t.norm();
  double a_norm = a.norm();
  double alpha = acos(f.dot(t)/t_norm); // dot product
  double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product
  double beta_plus = beta + px_error_angle;
  double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI
  double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines
  return (z_plus - z); // tau
}

static inline void writeSE3(YAMLWriter& writer, const SE3& value)
{
    writer.startFlowStyleListing();

    const Vector3& p = value.translation();
    writer.putScalar(p.x());
    writer.putScalar(p.y());
    writer.putScalar(p.z());

    const Quat& q = value.rotation();
    writer.putScalar(q.w());
    writer.putScalar(q.x());
    writer.putScalar(q.y());
    writer.putScalar(q.z());

    writer.endListing();
}

bool BodyItemImpl::store(Archive& archive)
{
    archive.setDoubleFormat("% .6f");

    archive.writeRelocatablePath("modelFile", self->filePath());
    archive.write("currentBaseLink", (currentBaseLink ? currentBaseLink->name() : ""), DOUBLE_QUOTED);

    /// \todo Improve the following for current / initial position representations
    write(archive, "rootPosition", body->rootLink()->p());
    write(archive, "rootAttitude", Matrix3(body->rootLink()->R()));
    Listing* qs = archive.createFlowStyleListing("jointPositions");
    int n = body->numJoints();
    for(int i=0; i < n; ++i){
        qs->append(body->joint(i)->q(), 10, n);
    }

    //! \todo replace the following code with the ValueTree serialization function of BodyState
    SE3 initialRootPosition;
    if(initialState.getRootLinkPosition(initialRootPosition)){
        write(archive, "initialRootPosition", initialRootPosition.translation());
        write(archive, "initialRootAttitude", Matrix3(initialRootPosition.rotation()));
    }
    BodyState::Data& initialJointPositions = initialState.data(BodyState::JOINT_POSITIONS);
    if(!initialJointPositions.empty()){
        qs = archive.createFlowStyleListing("initialJointPositions");
        for(int i=0; i < initialJointPositions.size(); ++i){
            qs->append(initialJointPositions[i], 10, n);
        }
    }

    write(archive, "zmp", zmp);

    if(isOriginalModelStatic != body->isStaticModel()){
        archive.write("staticModel", body->isStaticModel());
    }

    archive.write("selfCollisionDetection", isSelfCollisionDetectionEnabled);
    archive.write("isEditable", isEditable);

    return true;
}

void LiveSLAMWrapper::logCameraPose(const SE3& camToWorld, double time)
{
	Sophus::Quaternionf quat = camToWorld.unit_quaternion().cast<float>();
	Eigen::Vector3f trans = camToWorld.translation().cast<float>();

	char buffer[1000];
	int num = snprintf(buffer, 1000, "%f %f %f %f %f %f %f %f\n",
			time,
			trans[0],
			trans[1],
			trans[2],
			quat.x(),
			quat.y(),
			quat.z(),
			quat.w());

	if(outFile == 0)
		outFile = new std::ofstream(outFileName.c_str());
	outFile->write(buffer,num);
	outFile->flush();
}

 ForceTpl se3ActionInverse_impl(const SE3 & m) const
 {
   return ForceTpl(m.rotation().transpose()*linear_impl(),
     m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) );
 }

 /// af = aXb.act(bf)
 ForceTpl se3Action_impl(const SE3 & m) const
 {
   Vector3 Rf (static_cast<Vector3>( (m.rotation()) * linear_impl() ) );
   return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular_impl());
 }

Sim3 Sim3
::from_SE3(const SE3 & se3)
{
  return Sim3(ScSO3(1.,se3.so3()),se3.translation());
}

void SlamSystem::setReprojectionListRelateToLastestKeyFrame(int begin, int end, Frame* current,
                                                            const Eigen::Matrix3d& R_i_2_c, const Eigen::Vector3d& T_i_2_c )
{
    int num = end - begin;
    if ( num < 0 ){
        num += slidingWindowSize ;
    }
    int trackFrameCnt = 0 ;
    for (int i = 0; i < num; i++)
    {
        int ref_id = begin + i;
        if (ref_id >=  slidingWindowSize) {
            ref_id -= slidingWindowSize;
        }
        if ( slidingWindow[ref_id]->keyFrameFlag == false
             || trackFrameCnt > 10
             ){
            continue;
        }

        double closenessTH = 1.0 ;
        double distanceTH = closenessTH * 15 / (KFDistWeight*KFDistWeight);
        //double cosAngleTH = 1.0 - 0.25 * closenessTH ;

        //euclideanOverlapCheck
        double distFac = slidingWindow[ref_id]->meanIdepth ;
        Eigen::Vector3d dd = ( slidingWindow[ref_id]->T_bk_2_b0 - current->T_bk_2_b0) * distFac;
        if( dd.dot(dd) > distanceTH) continue;

//        Eigen::Quaterniond qq( slidingWindow[ref_id]->R_bk_2_b0.transpose() * current->R_bk_2_b0) ;
//        Eigen::Vector3d aa = qq.vec()*2.0 ;
//        double dirDotProd = aa.dot( aa ) ;
//        if(dirDotProd < cosAngleTH) continue;


        Matrix3d R_i_2_j ;
        Vector3d T_i_2_j ;
        SE3 c2f_init ;
        //check from current to ref
        R_i_2_j = slidingWindow[ref_id]->R_bk_2_b0.transpose() * current->R_bk_2_b0 ;
        T_i_2_j = -slidingWindow[ref_id]->R_bk_2_b0.transpose() * ( slidingWindow[ref_id]->T_bk_2_b0 - current->T_bk_2_b0 ) ;
        c2f_init.setRotationMatrix(R_i_2_j);
        c2f_init.translation() = T_i_2_j ;
        trackerConstraint->trackFrameOnPermaref(current, slidingWindow[ref_id].get(), c2f_init ) ;
        if ( trackerConstraint->trackingWasGood == false ){
            //ROS_WARN("first check fail") ;
            continue ;
        }
        //ROS_WARN("pass first check") ;

        //check from ref to current
        R_i_2_j = current->R_bk_2_b0.transpose() * slidingWindow[ref_id]->R_bk_2_b0 ;
        T_i_2_j = -current->R_bk_2_b0.transpose() * ( current->T_bk_2_b0 - slidingWindow[ref_id]->T_bk_2_b0 ) ;
        c2f_init.setRotationMatrix(R_i_2_j);
        c2f_init.translation() = T_i_2_j ;
        trackerConstraint->trackFrameOnPermaref(slidingWindow[ref_id].get(), current, c2f_init ) ;
        if ( trackerConstraint->trackingWasGood == false ){
            //ROS_WARN("second check fail") ;
            continue ;
        }
        //ROS_WARN("pass second check") ;

        //Pass the cross check
        if (  trackingReferenceConstraint->keyframe != slidingWindow[ref_id].get() ){
             trackingReferenceConstraint->importFrame( slidingWindow[ref_id].get() );
        }

        SE3 RefToFrame = trackerConstraint->trackFrame( trackingReferenceConstraint, current,
                                   c2f_init );
        trackFrameCnt++ ;
        //float tracking_lastResidual = trackerConstraint->lastResidual;
        //float tracking_lastUsage = trackerConstraint->pointUsage;
        //float tracking_lastGoodPerBad = trackerConstraint->lastGoodCount / (trackerConstraint->lastGoodCount + trackerConstraint->lastBadCount);
        float tracking_lastGoodPerTotal = trackerConstraint->lastGoodCount / (current->width(SE3TRACKING_MIN_LEVEL)*current->height(SE3TRACKING_MIN_LEVEL));
        Sophus::Vector3d dist = RefToFrame.translation() * slidingWindow[ref_id]->meanIdepth;
        float minVal = 1.0f;
        float lastTrackingClosenessScore = getRefFrameScore(dist.dot(dist), trackerConstraint->pointUsage, KFDistWeight, KFUsageWeight);
        if ( trackerConstraint->trackingWasGood == false
             ||  tracking_lastGoodPerTotal < MIN_GOODPERALL_PIXEL
             || lastTrackingClosenessScore > minVal
             )
        {
            continue ;
        }



#ifdef PROJECT_TO_IMU_CENTER
        Eigen::Matrix3d r_i_2_j = RefToFrame.rotationMatrix().cast<double>();
        Eigen::Vector3d t_i_2_j = RefToFrame.translation().cast<double>();
        Eigen::Matrix3d final_R = R_i_2_c.transpose()*r_i_2_j*R_i_2_c;
        Eigen::Vector3d final_T = R_i_2_c.transpose()*(r_i_2_j*T_i_2_c + t_i_2_j ) - R_i_2_c.transpose()*T_i_2_c ;
#else
        Eigen::Matrix3d final_R = RefToFrame.rotationMatrix().cast<double>();
        Eigen::Vector3d final_T = RefToFrame.translation().cast<double>();
#endif

        //ROS_WARN("[add link, from %d to %d]", slidingWindow[ref_id]->id(), current->id() ) ;
        insertCameraLink( slidingWindow[ref_id].get(), current,
                          final_R,
//.........这里部分代码省略.........