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;
}
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];
}
}
}
// 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;
}
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 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;
}
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();
}
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;
}
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
}
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);
}
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;
}
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;
}
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);
}
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();
}
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;
}
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();
}
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);
}
}
// 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
}
void ARDriver::Render(Image<Rgb<byte> >& imFrame, SE3<> se3CfromW) {
if (!mbInitialised) {
Init();
Reset();
};
mnCounter++;
// Upload the image to our frame texture
glBindTexture(GL_TEXTURE_RECTANGLE_ARB, mnFrameTex);
glTexSubImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, 0, 0, mirFrameSize.x,
mirFrameSize.y, GL_RGB, GL_UNSIGNED_BYTE, imFrame.data());
// Set up rendering to go the FBO, draw undistorted video frame into BG
glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, mnFrameBuffer);
CheckFramebufferStatus();
glViewport(0, 0, mirFBSize.x, mirFBSize.y);
DrawFBBackGround();
glClearDepth(1);
glClear(GL_DEPTH_BUFFER_BIT);
// Set up 3D projection
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMultMatrix(mCamera.MakeUFBLinearFrustumMatrix(0.005, 100));
glMultMatrix(se3CfromW);
DrawFadingGrid();
mGame.DrawStuff(se3CfromW.inverse().get_translation());
glDisable(GL_DEPTH_TEST);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glDisable(GL_BLEND);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// Set up for drawing 2D stuff:
glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
DrawDistortedFB();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
mGLWindow.SetupViewport();
mGLWindow.SetupVideoOrtho();
mGLWindow.SetupVideoRasterPosAndZoom();
}
void DisplayCallback(void)
{
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(_fovy, _aspect, _zNear, _zFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glMultMatrixd(_mvmatrix.GetArray());
DrawModel();
DrawTextInfo();
glutSwapBuffers();
glutPostRedisplay();
glClearColor(_backgroundcolor[0],_backgroundcolor[1],_backgroundcolor[2],_backgroundcolor[3]);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
static bool testExponential()
{
SE3<double> pose;
Eigen::Matrix4d S, expectedExp, closedForm;
S.setZero(); expectedExp.setZero(), closedForm.setZero();
SE3<double>::ParameterVectorType delta;
size_t n = 5000;
bool b = true;
while( n-- ){
delta[ 0 ] = Math::rand( -2.6, 2.6 );
delta[ 1 ] = Math::rand( -2.6, 2.6 );
delta[ 2 ] = Math::rand( -2.6, 2.6 );
delta[ 3 ] = Math::rand( -10.0, 10.0 );
delta[ 4 ] = Math::rand( -10.0, 10.0 );
delta[ 5 ] = Math::rand( -10.0, 10.0 );
fillExpMatrix( S, delta );
cvt::Math::exponential( S, expectedExp, 10 );
pose.evalExp( closedForm, delta );
bool res = ( ( expectedExp - closedForm ).array().abs().sum() / 12 < 0.00001 );
if( !res ){
std::cout << "Expected: \n" << expectedExp << std::endl;
std::cout << "Closed Form: \n" << closedForm << std::endl;
}
b &= res;
}
CVTTEST_PRINT( "exp()", b );
return b;
}
void MotionCallback(int x, int y)
{
bool bctrlpressed = false;
if (glutGetModifiers() == GLUT_ACTIVE_CTRL) {
bctrlpressed = true;
}
if ( _bRotateView ) {
Vec3 spoint = get_pos_sp(x, y);
Vec3 n = Cross(_spoint_prev, spoint);
if ( Norm(n) > 1e-6 ) {
if ( !bctrlpressed ) { n *= 3.0; }
Vec3 w = ~(_mvmatrix_prev.GetRotation()) * n;
_mvmatrix.SetRotation(_mvmatrix_prev.GetRotation() * Exp(w));
} else {
return;
}
}
if (_bTranslateView) { float den = 30; if ( bctrlpressed ) { den = 300; } _xcam += (float)(x-_downX)/den*_mscale; ycam += (float)(_downY-y)/den*_mscale; } // translate
if (_bZoomInOut) { float den = 30; if ( bctrlpressed ) { den = 300; } _sdepth -= (float)(_downY-y)/den*_mscale; } // zoom in/out
_downX = x; _downY = y;
glutPostRedisplay();
}
bool testForceSet()
{
using namespace se3;
typedef Eigen::Matrix<double,4,4> Matrix4;
typedef SE3::Matrix6 Matrix6;
typedef SE3::Vector3 Vector3;
typedef Force::Vector6 Vector6;
SE3 amb = SE3::Random();
SE3 bmc = SE3::Random();
SE3 amc = amb*bmc;
ForceSet F(12);
ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
F.block(10,2) = F2;
assert( F.matrix().col(10).norm() == 0.0 );
assert( isnan(F.matrix()(0,9)) );
std::cout << "F10 = " << F2.matrix() << std::endl;
std::cout << "F = " << F.matrix() << std::endl;
ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
ForceSet F4 = amb.act(F3);
SE3::Matrix6 aXb= amb;
assert( (aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix()) );
ForceSet bF = bmc.act(F3);
ForceSet aF = amb.act(bF);
ForceSet aF2 = amc.act(F3);
assert( aF.matrix().isApprox( aF2.matrix() ) );
ForceSet F36 = amb.act(F3.block(3,6));
assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix()) );
ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6))
.isApprox(F36full.matrix().block(0,3,6,6)) );
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 bool testApply()
{
bool b, ret;
// apply delta:
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
Eigen::Matrix<double, 4, 4> diff;
SE3<double> pose;
pose.set( delta );
delta[ 0 ] = Math::deg2Rad( 1.5 );
delta[ 1 ] = Math::deg2Rad( 1.1 );
delta[ 2 ] = Math::deg2Rad( 1.6 );
delta[ 3 ] = 1;
delta[ 4 ] = 1;
delta[ 5 ] = 1;
pose.apply( delta );
expectedT( 0, 3 ) = delta[ 3 ];
expectedT( 1, 3 ) = delta[ 4 ];
expectedT( 2, 3 ) = delta[ 5 ];
Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
double angle = axis.norm(); axis /= angle;
expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
diff = expectedT - pose.transformation();
ret = b = ( diff.array().abs().sum() / 12 < 0.001 );
if( !b ){
std::cout << expectedT << std::endl;
std::cout << pose.transformation() << std::endl;
std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
}
pose.apply( -delta );
expectedT.setIdentity();
b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
CVTTEST_PRINT( "apply", b );
ret &= b;
return ret;
}
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);
// Read ratios
double c_over_f = GV3::get<double>("CanonicalImage.LeftCOverF");
double f_over_c = 1.0 / c_over_f;
// Transfer some points
int horizon_y = image.ny()/2;
for (int j = 0; j < 50; j++) {
Vector<2> im_p1 = makeVector(rand()%image.nx(), rand()%image.ny());
Vector<2> ret_p1 = cam.UnProject(im_p1);
double y2;
if (ret_p1[1] > 0) {
y2 = -c_over_f * ret_p1[1];
} else {
y2 = -f_over_c * ret_p1[1];
}
Vector<2> ret_p2 = makeVector(ret_p1[0], y2);
Vector<2> im_p2 = cam.Project(ret_p2);
Vector<3> orig_ret_p1 = R_l * unproject(ret_p1);
Vector<3> orig_ret_p2 = R_l * unproject(ret_p2);
Vector<2> orig_im_p1 = cam.Project(project(orig_ret_p1));
Vector<2> orig_im_p2 = cam.Project(project(orig_ret_p2));
DrawLineClipped(canvas, orig_im_p1, orig_im_p2, BrightColors::Get(j));
DrawLineClipped(canvas_l, im_p1, im_p2, BrightColors::Get(j));
}
string basename = "out/canon"+PaddedInt(id, 4);
WriteImage(basename+"_left.png", canvas_l);
WriteImage(basename+"_right.png", canvas_r);
WriteImage(basename+"_orig.png", canvas);
}
}
}
}
/// 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());
}
ForceTpl se3ActionInverse_impl(const SE3 & m) const
{
return ForceTpl(m.rotation().transpose()*linear_impl(),
m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) );
}
