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,
final_T,
MatrixXd::Identity(6, 6)*DENSE_TRACKING_WEIGHT ) ;
break ;
}
}
