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

    // for EKF, we publish Tcw
    void publishPosewithCovariance(ros::Time stamp)
    {
//        SE3<> camPose = tracker_->GetCurrentPose();//Tcw
        SE3<> camPose = camPose4pub;//Tcw

        if (true) {
            geometry_msgs::PoseWithCovarianceStampedPtr pose(new geometry_msgs::PoseWithCovarianceStamped);//Tcw
            pose->header.stamp = stamp;
            pose->header.frame_id = "/ptam_world_cov";

            pose->pose.pose.position.x = camPose.get_translation()[0];
            pose->pose.pose.position.y = camPose.get_translation()[1];
            pose->pose.pose.position.z = camPose.get_translation()[2];
            quat_from_so3(pose->pose.pose.orientation, camPose.get_rotation());

            TooN::Matrix<6> covar = tracker_->GetPoseCovariance();
            for (unsigned int i = 0; i < pose->pose.covariance.size(); i++){

                pose->pose.covariance[i] = sqrt(fabs(covar[i % 6][i / 6]));
//                cout << pose->pose.covariance[i] << ", ";
            }
            pose->pose.covariance[1] = tracker_->GetCurrentKeyFrame().dSceneDepthMedian;

//            cout << "PTAM pose, Tcw: " << endl;
//            cout << pose->pose.pose.position.x << ", " << pose->pose.pose.position.y << ", " << pose->pose.pose.position.z << endl;
//            cout << pose->pose.pose.orientation.w << ", " << pose->pose.pose.orientation.x << ", "
//                 << pose->pose.pose.orientation.y << ", " << pose->pose.pose.orientation.z << endl;

            SE3<> roboPose = se3IMUfromcam * camPose;//Twi = (Tic * Tcw).inv
            roboPose = roboPose.inverse();
            cout << "POSITION DIFF: "<< PoseEKF_wi.get_translation()[0]-roboPose.get_translation()[0] << ", " <<
                    PoseEKF_wi.get_translation()[1] - roboPose.get_translation()[1]<< ", " <<
                    PoseEKF_wi.get_translation()[2] - roboPose.get_translation()[2] << endl;
            geometry_msgs::Quaternion quatRobo;
            quat_from_so3(quatRobo, roboPose.get_rotation());//Riw
//            cout << "PTAM pose, Twi: " << endl;
//            cout << roboPose.get_translation()[0] << ", " << roboPose.get_translation()[1] << ", " << roboPose.get_translation()[2] << endl;
//            cout << quatRobo.w << ", " << quatRobo.x << ", "
//                 << quatRobo.y << ", " << quatRobo.z << endl;


//            cout << "Pose Covariance: " << endl;
//            cout << pose->pose.covariance[0] << ", " << pose->pose.covariance[1] << ", " << pose->pose.covariance[2] << endl;
//            cout << pose->pose.covariance[3] << ", " << pose->pose.covariance[4] << ", " << pose->pose.covariance[5] << endl;
//            cout << pose->pose.covariance[6] << ", " << pose->pose.covariance[7] << ", " << pose->pose.covariance[8] << endl;

            cam_posewithcov_pub_.publish(pose);

            geometry_msgs::TransformStampedPtr quadpose(new geometry_msgs::TransformStamped);// for ros nodelets, publish ptr.
            quadpose->header.stamp = stamp;
            quadpose->header.frame_id = "/ptam_world";
            quadpose->transform.translation.x = roboPose.get_translation()[0];
            quadpose->transform.translation.y = roboPose.get_translation()[1];
            quadpose->transform.translation.z = roboPose.get_translation()[2];
            quat_from_so3(quadpose->transform.rotation, roboPose.get_rotation());
            quad_pose_pub_.publish(quadpose);

            logPose(stamp, roboPose);
        }
    }

    // get Tcw
    SE3<> imu2camWorld(pximu::AttitudeData attitude_data){
        Matrix<3> Rroll =  Data(1.0, 0, 0,//roll Rot(-roll)T=Rot(roll)
                              0, cos(attitude_data.roll), -sin(attitude_data.roll),
                              0, sin(attitude_data.roll), cos(attitude_data.roll));
        Matrix<3> Rpitch = Data(cos(attitude_data.pitch), 0, -sin(attitude_data.pitch),//pitch Rot(-pitch)T=Rot(pitch)
                              0, 1.0, 0,
                              sin(attitude_data.pitch), 0, cos(attitude_data.pitch));

        Matrix<3> roll = Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in
                              0, -1, 0, // a world frame which pointing downward.
                              0, 0, -1);

        SE3<> camWorld = SE3<>();
        Matrix<3> rotation = roll * Rpitch * Rroll; //Rwr*Rrc-->Rwc
        camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();

        Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc
        Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation();
        camWorld.get_translation()[0] = 0.0;//twc[0]; //twc
        camWorld.get_translation()[1] = 0.0;//twc[1]; // TODO: find out the bug why must use 0
        camWorld.get_translation()[2] = twc[2];

        camWorld = camWorld.inverse();//Tcw

        cout<< "TCW INITIALIZED FROM IMU ATT. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl;
        return camWorld;
    }

    SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose
        Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i
        Matrix<3> iniOrientationEKF;
        iniOrientationEKF = tag::quaternionToMatrix(attivec);

        Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in
                              0, -1, 0, // a world frame which pointing downward.
                              0, 0, -1);

        SE3<> camWorld = SE3<>();
        Matrix<3> rotation;
        if (tracker_->attitude_get)
            rotation = iniOrientationEKF; //
        else
            rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i
        camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric)

        Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc
        Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation();
        camWorld.get_translation()[0] = 0.0;//twc[0]; //twc
        camWorld.get_translation()[1] = 0.0;//twc[1];
        camWorld.get_translation()[2] = twc[2];

        camWorld = camWorld.inverse();//Tcw

        cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl;
//        cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl;
        return camWorld;
    }

string serializeForOutput(SE3<double> pose)
{
  stringstream so;
  Vector<3>& trans=pose.get_translation();
  Matrix<3, 3> rot=pose.get_rotation().get_matrix(); // row major
  // output in column-major - for matlab! just use reshape(x,3,4)
  for (uint32_t j = 0; j < 3; ++j)
    for (uint32_t i = 0; i < 3; ++i)
      so << rot(i, j) << "\t";
  for (uint32_t i = 0; i < 3; ++i)
      so << trans[i] << "\t";
  return so.str();
}

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

#if 0
    SE3<> id(makeVector(1,0,0,0,0,0));
    
    const SE3<F<double> > g = make_left_fad_se3(id);
    for(int i = 0; i < 6; ++i)
        cout << get_derivative(g.get_rotation().get_matrix(), i) << get_derivative(g.get_translation(), i) <<  "\n\n";

    Vector<3> in = makeVector(1,2,3);
    const Vector<3, F<double> > p = g * in;
    cout << p << "\n" << get_jacobian<3,6>(p) << endl;
#endif
}

    // the second camera pose in the master camera frames
    SE3<> Load_camsec_para()
    {
        SE3<> campara;
        ifstream cam_para_table;
        string table_path = cam_para_path_second;
        double temp1, temp2, temp3, temp4, temp5;
        cam_para_table.open(table_path.c_str());
        assert(cam_para_table.is_open());

//        cam_para_table>>temp1>>temp2>>temp3>>temp4>>temp5;
        Matrix<3> cam;
        for (int i = 0; i < 3; i ++){
            for (int j = 0; j < 3; j ++){
                cam_para_table>>temp1;
                cam(i, j) = temp1;
            }
            cam_para_table>>temp1;
            campara.get_translation()[i] = temp1/1000.0;
        }
        campara.get_rotation() = cam;

        return campara;//Tic
    }

int main(int argc, char **argv) {
	InitVars(argc, argv);
	if (argc != 1) {
		DLOG << "Usage: "<<argv[0];
		return 0;
	}

	// Load the map and rotate to canonical image
	Map map;
	map.load_images = false;
	map.Load();

	Vector<3> lnR = makeVector(1.1531, 1.26237, -1.24435);  // for lab_kitchen1

	SE3<> scene_to_slam;
	scene_to_slam.get_rotation() = SO3<>::exp(lnR);

	map.scene_to_slam = SO3<>::exp(lnR);

	// TODO: map.Load() should do this automatically
	map.undistorter.Compute(ImageRef(640,480));
	ptam::ATANCamera& cam = map.undistorter.cam;

	vector<int> frame_ids;
	ParseMultiRange(GV3::get<string>("CanonicalImages.InputFrames"), frame_ids);

	COUNTED_FOREACH(int i, int id, frame_ids) {
		if (id < 0 || id >= map.frame_specs.size()) {
			DLOG << "Frame index " << id << " out of range"
					 << " (there are " << map.frame_specs.size() << "frames available)";
		} else {
			const Frame& fs = map.frame_specs[frame_ids[i]];
			if (!fs.lost) {


				// Read the image and detect lines
				ImageBundle image(fs.filename);

				PeakLines lines;
	 			lines.Compute(image, fs.pose, map);
				
				SE3<> pose = fs.pose * scene_to_slam;


				// Project vanishing points
				Vector<3> vpts[3];
				int vert_axis = 0;
				for (int i = 0; i < 3; i++) {
					vpts[i] = pose.get_rotation().get_matrix().T()[i];
					if (abs(vpts[i][1]) > abs(vpts[vert_axis][1])) {
						vert_axis = i;
					}
				}

				int l_axis = (vert_axis+1)%3;
				int r_axis = (vert_axis+2)%3;

				// Compute the rotations
				Matrix<3> R_l_inv, R_r_inv;

				R_l_inv.T()[0] = positive(vpts[l_axis], 0);
				R_l_inv.T()[1] = positive(vpts[vert_axis], 1);
				R_l_inv.T()[2] = vpts[r_axis];

				R_r_inv.T()[0] = positive(vpts[r_axis], 0);
				R_r_inv.T()[1] = positive(vpts[vert_axis], 1);
				R_r_inv.T()[2] = vpts[l_axis];

				Matrix<3> R_l = LU<3>(R_l_inv).get_inverse();
				Matrix<3> R_r = LU<3>(R_r_inv).get_inverse();

				// Warp the images
				ImageRGB<byte> canvas_l(image.sz()), canvas_r(image.sz());
				canvas_l.Clear(PixelRGB<byte>(255,255,255));
				canvas_r.Clear(PixelRGB<byte>(255,255,255));
				for (int y = 0; y < image.ny(); y++) {
					for (int x = 0; x < image.nx(); x++) {

						Vector<3> dest = unproject(cam.UnProject(makeVector(x,y)));

						Vector<3> l_src = R_l_inv * dest;
						Vector<3> r_src = R_r_inv * dest;

						Vector<2> l_im = cam.Project(project(l_src));
						Vector<2> r_im = cam.Project(project(r_src));

						ImageRef l_ir = round_pos(l_im);
						ImageRef r_ir = round_pos(r_im);

						if (image.contains(l_ir)) {
							canvas_l[y][x] = image.rgb[l_ir];
						}
						if (image.contains(r_ir)) {
							canvas_r[y][x] = image.rgb[r_ir];
						}
					}
				}

				ImageRGB<byte> canvas;
				ImageCopy(image.rgb, canvas);
//.........这里部分代码省略.........