本文整理汇总了C++中SE3类的典型用法代码示例。如果您正苦于以下问题：C++ SE3类的具体用法？C++ SE3怎么用？C++ SE3使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了SE3类的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的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;
}
开发者ID:bigjun，项目名称:idSLAM，代码行数:29，代码来源:DualMonoSLAMnodelet.cpp
示例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];
}
}
}
开发者ID:hjhdog1，项目名称:ConcentricTubes，代码行数:25，代码来源:SyntheticDataGenerator.cpp
示例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;
}
开发者ID:bigjun，项目名称:idSLAM，代码行数:28，代码来源:DualMonoSLAMnodelet.cpp
示例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;
}
开发者ID:zamzami，项目名称:pinocchio，代码行数:8，代码来源:joint-spherical.hpp
示例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;
}
开发者ID:MaximilienNaveau，项目名称:pinocchio，代码行数:8，代码来源:joint-revolute-unaligned.hpp
示例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;
}
开发者ID:florent-lamiraux，项目名称:pinocchio，代码行数:11，代码来源:joint-planar.hpp
示例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;
}
开发者ID:jing-vision，项目名称:lsd_slam_windows，代码行数:52，代码来源:DebugOutput3DWrapper.cpp
示例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();
}
开发者ID:jstraub，项目名称:ptamRosBinaryFeatureRelocalization，代码行数:13，代码来源:results.cpp
示例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;
}
开发者ID:SamuelDudley，项目名称:rpg_svo，代码行数:14，代码来源:matcher.cpp
示例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
}
开发者ID:AsherBond，项目名称:MondocosmOS，代码行数:15，代码来源:fadbad.cpp
示例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);
}
开发者ID:nttputus，项目名称:ScaViSLAM，代码行数:27，代码来源:slam_graph-impl.cpp
示例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;
}
开发者ID:zamzami，项目名称:pinocchio，代码行数:7，代码来源:joint-prismatic.hpp
示例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;
}
开发者ID:dengchengcheng，项目名称:slambook，代码行数:60，代码来源:dense_mapping.cpp
示例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);
}
开发者ID:skrogh，项目名称:lsd_slam，代码行数:47，代码来源:ROSOutput3DWrapper.cpp
示例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();
}
开发者ID:kayusawa，项目名称:choreonoid，代码行数:17，代码来源:MultiSE3Seq.cpp
示例16: third
Matrix6d third(const SE3 & A, const Vector6d & d)
{
const Matrix6d & AdjA = A.Adj();
Matrix6d d_lie = SE3::d_lieBracketab_by_d_a(d);
//cerr << d_lie << endl;
return AdjA + 0.5*d_lie*AdjA + (1./12.)*d_lie*d_lie*AdjA;
}
开发者ID:asimay，项目名称:ScaViSLAM_ros，代码行数:8，代码来源:anchored_points.cpp
示例17: write
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;
}
开发者ID:orikuma，项目名称:choreonoid，代码行数:41，代码来源:BodyItem.cpp
示例18: logCameraPose
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();
}
开发者ID:IceFish99，项目名称:lsd_slam，代码行数:21，代码来源:LiveSLAMWrapper.cpp
示例19: visualizeMarkers
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);
}
}
开发者ID:ruffsl，项目名称:rpg_svo，代码行数:38，代码来源:visualizer.cpp
示例20: Load_camsec_para
// 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
}
开发者ID:bigjun，项目名称:idSLAM，代码行数:24，代码来源:DualMonoSLAMnodelet.cpp
注：本文中的SE3类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。 |
客服电话
APP下载
官方微信
请发表评论