本文整理汇总了C++中SE3类的典型用法代码示例。如果您正苦于以下问题：C++ SE3类的具体用法？C++ SE3怎么用？C++ SE3使用的例子？那么, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了SE3类的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: IMU2camWorldfromQuat
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;
}示例2: GenerateTipTrajectory
void SyntheticDataGenerator::GenerateTipTrajectory()
{
this->tipTrajectory.resize(this->jointTrajectory.size());
SE3 tipFrame;
for(int i = 0 ; i < this->jointTrajectory.size(); ++i)
{
double* rotation = this->jointTrajectory[i].data() + 1;
double* translation = this->jointTrajectory[i].data() + 1 + this->robot->GetNumOfTubes();
//if(kinematics->ComputeKinematics(rotation, translation))
//std::cout << "Solved!" << std::endl;
if(!kinematics->ComputeKinematics(rotation, translation))
std::cout << rotation[0] << "\t" << rotation[1] << "\t" << rotation[2] << "\t" << translation[0] << "\t" << translation[1] << "\t" << translation[2] << std::endl;
kinematics->GetBishopFrame(tipFrame);
this->tipTrajectory[i].resize(12);
for(int j = 0; j < 3; ++j)
{
this->tipTrajectory[i][j] = tipFrame.GetPosition()[j];
this->tipTrajectory[i][j+3] = tipFrame.GetOrientation().GetX()[j];
this->tipTrajectory[i][j+6] = tipFrame.GetOrientation().GetY()[j];
this->tipTrajectory[i][j+9] = tipFrame.GetOrientation().GetZ()[j];
}
}
}示例3: imu2camWorld
// 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;
}示例4: se3Action
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;
}示例5: se3Action
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;
}示例6: se3Action
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;
}示例7: memcpy
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;
}示例8: serializeForOutput
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();
}示例9: depthFromTriangulation
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;
}示例10: main
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
}示例11: G2oEdgeSE3
void SlamGraph<SE3,StereoCamera, SE3XYZ_STEREO, 3>
::addConstraintToG2o(const SE3 & T_2_from_1,
const Matrix6d &
Lambda_2_from_1,
int pose_id_1,
int pose_id_2,
g2o::SparseOptimizer * optimizer)
{
G2oEdgeSE3 * e = new G2oEdgeSE3();
g2o::HyperGraph::Vertex * pose1_vertex
= GET_MAP_ELEM(pose_id_1, optimizer->vertices());
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose1_vertex);
g2o::HyperGraph::Vertex * pose2_vertex
= GET_MAP_ELEM(pose_id_2, optimizer->vertices());
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose2_vertex);
e->measurement() = T_2_from_1;
e->inverseMeasurement() = T_2_from_1.inverse();
e->information() = Lambda_2_from_1;
optimizer->addEdge(e);
}示例12: se3Action
Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
{
Eigen::Matrix<double,6,1> res;
res.head<3>() = m.rotation().col(axis);
res.tail<3>() = Motion::Vector3::Zero();
return res;
}示例13: updateDepthFilter
bool updateDepthFilter(
const Vector2d& pt_ref,
const Vector2d& pt_curr,
const SE3& T_C_R,
Mat& depth,
Mat& depth_cov
)
{
// 我是一只喵
// 不知道这段还有没有人看
// 用三角化计算深度
SE3 T_R_C = T_C_R.inverse();
Vector3d f_ref = px2cam( pt_ref );
f_ref.normalize();
Vector3d f_curr = px2cam( pt_curr );
f_curr.normalize();
// 方程参照本书第 7 讲三角化一节
Vector3d t = T_R_C.translation();
Vector3d f2 = T_R_C.rotation_matrix() * f_curr;
Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );
double A[4];
A[0] = f_ref.dot ( f_ref );
A[2] = f_ref.dot ( f2 );
A[1] = -A[2];
A[3] = - f2.dot ( f2 );
double d = A[0]*A[3]-A[1]*A[2];
Vector2d lambdavec =
Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
-A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
Vector3d xm = lambdavec ( 0,0 ) * f_ref;
Vector3d xn = t + lambdavec ( 1,0 ) * f2;
Vector3d d_esti = ( xm+xn ) / 2.0; // 三角化算得的深度向量
double depth_estimation = d_esti.norm(); // 深度值
// 计算不确定性（以一个像素为误差）
Vector3d p = f_ref*depth_estimation;
Vector3d a = p - t;
double t_norm = t.norm();
double a_norm = a.norm();
double alpha = acos( f_ref.dot(t)/t_norm );
double beta = acos( -a.dot(t)/(a_norm*t_norm));
double beta_prime = beta + atan(1/fx);
double gamma = M_PI - alpha - beta_prime;
double p_prime = t_norm * sin(beta_prime) / sin(gamma);
double d_cov = p_prime - depth_estimation;
double d_cov2 = d_cov*d_cov;
// 高斯融合
double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2);
double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 );
depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse;
depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2;
return true;
}示例14: memcpy
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);
}示例15: writeSE3
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();
}