// 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;
}
// 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);
}
}
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
}
