int problem3_main(){
std::ofstream fout1("problem3.30.txt");
solve(30.0 / 180.0 * acos(-1), fout1);
fout1.close();
std::ofstream fout2("problem3.40.txt");
solve(40.0 / 180.0 * acos(-1), fout2);
fout2.close();
std::ofstream fout3("problem3.50.txt");
solve(50.0 / 180.0 * acos(-1), fout3);
fout3.close();
B2_M = 0.0;
std::ofstream fout4("problem3-no-air.30.txt");
solve(30.0 / 180.0 * acos(-1), fout4);
fout4.close();
std::ofstream fout5("problem3-no-air.40.txt");
solve(40.0 / 180.0 * acos(-1), fout5);
fout5.close();
std::ofstream fout6("problem3-no-air.50.txt");
solve(50.0 / 180.0 * acos(-1), fout6);
fout6.close();
return 0;
}
int main()
{
std::ofstream fout1("test1.dat"); // Flat Sky
std::ofstream fout2("test2.dat"); // Spherical near equator
std::ofstream fout3("test3.dat"); // Spherical around N pole.
std::ofstream fout4("test4.dat"); // Test1 with random offsets
std::ofstream fout4c("test4_centers.dat"); // Corresponding centers for test4.dat
fout1.precision(12);
fout2.precision(12);
fout3.precision(12);
fout4.precision(12);
fout4c.precision(12);
double ra0 = 0.; // RA, Dec of center for test2.
double dec0 = 80.;
const long ngal = 100000;
const double rscale = 1.; // Scale radius in degrees.
const double rmax = 5.; // In units of rscale
// The functional form imprinted is purely radial:
// gamma_t(r) = gamma0/r
// kappa(r) = kappa0/r
// n(r) ~ exp(-r^2/2)
// where r is in units of rscale
const double gamma0 = 1.e-3;
const double kappa0 = 3.e-3;
// Make sure gamma_t < 0.5
const double rmin = gamma0/0.5;
srand(1234); // To make it deterministic.
// Normally for the Box-Muller Transformation, one would restrict rsq < 1
// But we then want to clip the resulting distribution at rmax, so
// rsq * fac^2 < rmax^2
// rsq * (-2.*log(rsq)/rsq) < rmax^2
// -2. * log(rsq) < rmax^2
// rsq > exp(-rmax^2/2)
const double rsqmin = exp(-rmax*rmax/2.);
const double rsqmax = exp(-rmin*rmin/2.);
//std::cout<<"rsq min/max = "<<rsqmin<<','<<rsqmax<<std::endl;
double xdeg_min=0., xdeg_max=0., var_xdeg=0.;
double ydeg_min=0., ydeg_max=0., var_ydeg=0.;
for (long n=0; n<ngal; ++n) {
double x,y,rsq;
do {
x = double(rand()) / RAND_MAX; // Random number from 0..1
y = double(rand()) / RAND_MAX;
//std::cout<<"x,y = "<<x<<','<<y<<std::endl;
x = 2.*x-1.; // Now from -1..1
y = 2.*y-1.;
//std::cout<<"x,y => "<<x<<','<<y<<std::endl;
rsq = x*x+y*y;
//std::cout<<"rsq = "<<rsq<<std::endl;
} while (rsq <= rsqmin || rsq >= rsqmax);
// Use Box-Muller Transformation to convert to Gaussian distribution in x,y.
double fac = sqrt(-2.*log(rsq)/rsq);
x *= fac;
y *= fac;
double r = fac*sqrt(rsq);
double theta = atan2(y,x);
double g = gamma0 / r;
double k = kappa0 / r;
assert(g < 0.5);
double xdeg = x*rscale;
double ydeg = y*rscale;
double rdeg = r*rscale;
// Do some sanity checks:
if (xdeg < xdeg_min) xdeg_min = xdeg;
if (xdeg > xdeg_max) xdeg_max = xdeg;
if (ydeg < ydeg_min) ydeg_min = ydeg;
if (ydeg > ydeg_max) ydeg_max = ydeg;
var_xdeg += xdeg*xdeg;
var_ydeg += ydeg*ydeg;
//
// Flat sky:
//
double g1 = -g * cos(2.*theta);
double g2 = -g * sin(2.*theta);
fout1 << xdeg <<" "<< ydeg <<" "<< g1 <<" "<< g2 <<" "<< k <<std::endl;
// With offsets in position:
double dx = 2.*double(rand()) / RAND_MAX - 1.; // Random number from -1..1
double dy = 2.*double(rand()) / RAND_MAX - 1.;
dx *= rscale;
dy *= rscale;
fout4 << xdeg + dx <<" "<< ydeg + dy <<" "<< g1 <<" "<< g2 <<" "<< k <<std::endl;
fout4c << dx <<" "<< dy <<" 1. "<<std::endl;
//
// Spherical near equator:
//
// Use spherical triangle with A = point, B = (ra0,dec0), C = N. pole
// a = Pi/2-dec0
// c = 2*atan(r/2)
// B = Pi/2 - theta
// Solve the rest of the triangle with spherical trig:
double c = 2.*atan( (rdeg*M_PI/180.) / 2.);
double a = M_PI/2. - (dec0*M_PI/180.);
double B = x > 0 ? M_PI/2. - theta : theta - M_PI/2.;
if (B < 0) B += 2.*M_PI;
if (B > 2.*M_PI) B -= 2.*M_PI;
double cosb = cos(a)*cos(c) + sin(a)*sin(c)*cos(B);
double b = std::abs(cosb) < 1. ? acos(cosb) : 0.;
double cosA = (cos(a) - cos(b)*cos(c)) / (sin(b)*sin(c));
double A = std::abs(cosA) < 1. ? acos(cosA) : 0.;
double cosC = (cos(c) - cos(a)*cos(b)) / (sin(a)*sin(b));
double C = std::abs(cosC) < 1. ? acos(cosC) : 0.;
//std::cout<<"x,y = "<<x<<','<<y<<std::endl;
//std::cout<<"a = "<<a<<std::endl;
//std::cout<<"b = "<<b<<std::endl;
//std::cout<<"c = "<<c<<std::endl;
//std::cout<<"A = "<<A<<std::endl;
//std::cout<<"B = "<<B<<std::endl;
//std::cout<<"C = "<<C<<std::endl;
// Compute ra,dec from these.
// Note: increasing x is decreasing ra. East is left on the sky!
double ra = x>0 ? -C : C;
double dec = M_PI/2. - b;
ra *= 180. / M_PI;
dec *= 180. / M_PI;
ra += ra0;
//std::cout<<"ra = "<<ra<<std::endl;
//std::cout<<"dec = "<<dec<<std::endl;
// Rotate shear relative to local west
std::complex<double> gamma(g1,g2);
double beta = M_PI - (A+B);
if (x > 0) beta = -beta;
//std::cout<<"gamma = "<<gamma<<std::endl;
//std::cout<<"beta = "<<beta<<std::endl;
std::complex<double> exp2ibeta(cos(2.*beta),sin(2.*beta));
gamma *= exp2ibeta;
//std::cout<<"gamma => "<<gamma<<std::endl;
fout2 << ra <<" "<< dec <<" "<< real(gamma) <<" "<<imag(gamma) <<" "<<k <<std::endl;
//
// Spherical around N pole
//
dec = 90. - c * 180./M_PI;
ra = theta * 12. / M_PI;
fout3 << ra <<" "<< dec <<" "<< g <<" "<<0. <<" "<<k <<std::endl;
}
var_xdeg /= ngal;
var_ydeg /= ngal;
std::cout<<"Min/Max x = "<<xdeg_min<<" "<<xdeg_max<<std::endl;;
std::cout<<"Min/Max y = "<<ydeg_min<<" "<<ydeg_max<<std::endl;;
std::cout<<"sqrt(Var(x)) = "<<sqrt(var_xdeg)<<std::endl;
std::cout<<"sqrt(Var(y)) = "<<sqrt(var_ydeg)<<std::endl;
// Make random catalogs
std::ofstream foutr1("rand1.dat");
std::ofstream foutr2("rand2.dat");
std::ofstream foutr3("rand3.dat");
foutr1.precision(12);
foutr2.precision(12);
foutr3.precision(12);
xdeg_min=xdeg_max=var_xdeg=0.;
ydeg_min=ydeg_max=var_ydeg=0.;
for (long n=0; n<10*ngal; ++n) {
double x,y,rsq;
do {
x = double(rand()) / RAND_MAX; // Random number from 0..1
y = double(rand()) / RAND_MAX;
x = 2.*x-1.; // Now from -1..1
y = 2.*y-1.;
rsq = x*x+y*y;
} while (rsq >= 1.);
x *= rmax;
y *= rmax;
double r = rmax*sqrt(rsq);
double theta = atan2(y,x);
double xdeg = x*rscale;
double ydeg = y*rscale;
double rdeg = r*rscale;
// Do some sanity checks:
if (xdeg < xdeg_min) xdeg_min = xdeg;
if (xdeg > xdeg_max) xdeg_max = xdeg;
if (ydeg < ydeg_min) ydeg_min = ydeg;
if (ydeg > ydeg_max) ydeg_max = ydeg;
var_xdeg += xdeg*xdeg;
var_ydeg += ydeg*ydeg;
//
// flat sky:
//
foutr1 << xdeg <<" "<< ydeg << std::endl;
//
// Spherical near equator:
//
double c = 2.*atan( (rdeg*M_PI/180.) / 2.);
double a = M_PI/2. - (dec0*M_PI/180.);
double B = x > 0 ? M_PI/2. - theta : theta - M_PI/2.;
if (B < 0) B += 2.*M_PI;
if (B > 2.*M_PI) B -= 2.*M_PI;
double cosb = cos(a)*cos(c) + sin(a)*sin(c)*cos(B);
double b = std::abs(cosb) < 1. ? acos(cosb) : 0.;
double cosC = (cos(c) - cos(a)*cos(b)) / (sin(a)*sin(b));
double C = std::abs(cosC) < 1. ? acos(cosC) : 0.;
double ra = x>0 ? -C : C;
double dec = M_PI/2. - b;
ra *= 180. / M_PI;
dec *= 180. / M_PI;
ra += ra0;
foutr2 << ra <<" "<< dec <<std::endl;
//
// Spherical around N pole
//
dec = 90. - c * 180./M_PI;
ra = theta * 12. / M_PI;
foutr3 << ra <<" "<< dec <<std::endl;
}
var_xdeg /= ngal;
var_ydeg /= ngal;
std::cout<<"For randoms:\n";
std::cout<<"Min/Max x = "<<xdeg_min<<" "<<xdeg_max<<std::endl;;
std::cout<<"Min/Max y = "<<ydeg_min<<" "<<ydeg_max<<std::endl;;
std::cout<<"sqrt(Var(x)) = "<<sqrt(var_xdeg)<<std::endl;
std::cout<<"sqrt(Var(y)) = "<<sqrt(var_ydeg)<<std::endl;
return 0;
}
int main(int argc, char * argv[])
{
int width = 640;
int height = 480;
Resolution::getInstance(width, height);
Intrinsics::getInstance(528, 528, 320, 240);
cv::Mat intrinsicMatrix = cv::Mat(3,3,CV_64F);
intrinsicMatrix.at<double>(0,0) = Intrinsics::getInstance().fx();
intrinsicMatrix.at<double>(1,1) = Intrinsics::getInstance().fy();
intrinsicMatrix.at<double>(0,2) = Intrinsics::getInstance().cx();
intrinsicMatrix.at<double>(1,2) = Intrinsics::getInstance().cy();
intrinsicMatrix.at<double>(0,1) =0;
intrinsicMatrix.at<double>(1,0) =0;
intrinsicMatrix.at<double>(2,0) =0;
intrinsicMatrix.at<double>(2,1) =0;
intrinsicMatrix.at<double>(2,2) =1;
Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2];
IplImage * deCompImage = 0;
std::string logFile="/home/lili/Kinect_Logs/2015-11-05.00.klg";
// assert(pcl::console::parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file");
RawLogReader logReader(decompressionBuffer,
deCompImage,
logFile,
true);
cv::Mat1b tmp(height, width);
cv::Mat3b depthImg(height, width);
PlaceRecognition placeRecognition(&intrinsicMatrix);
iSAMInterface iSAM;
//Keyframes
KeyframeMap map(true);
Eigen::Vector3f lastPlaceRecognitionTrans = Eigen::Vector3f::Zero();
Eigen::Matrix3f lastPlaceRecognitionRot = Eigen::Matrix3f::Identity();
int64_t lastTime = 0;
OdometryProvider * odom = 0;
//int frame_index = 0;
// uint64_t timestamp;
/*if(true)
{
odom = new FOVISOdometry;
if(logReader.hasMore())
{
logReader.getNext();
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
}*/
//else
// {
odom = new DVOdometry;
if(logReader.hasMore())
{
logReader.getNext();
DVOdometry * dvo = static_cast<DVOdometry *>(odom);
dvo->firstRun((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
//}
ofstream fout1("camera_pose_DVOMarch28.txt");
ofstream fout2("camera_pose_KeyframeMotionMetric0.1March28.txt");
ofstream fout3("loop_closure_transformationMarch28.txt");
ofstream fout4("camera_pose_after_optimizationMarch28.txt");
ofstream fout5("camera_pose_after_optimizationMarch28DVOCov.txt");
ofstream fout6("camera_pose_after_optimizationMarch28DVOLoopTransCov.txt");
/*
pcl::visualization::PCLVisualizer cloudViewer;
cloudViewer.setBackgroundColor(1, 1, 1);
cloudViewer.initCameraParameters();
cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
*/
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
int loopClosureCount=0;
while(logReader.hasMore())
{
logReader.getNext();
cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]);
cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0);
cv::cvtColor(tmp, depthImg, CV_GRAY2RGB);
cv::imshow("RGB", rgbImg);
cv::imshow("Depth", depthImg);
char key = cv::waitKey(1);
if(key == 'q')
{
break;
}
else if(key == ' ')
{
key = cv::waitKey(0);
}
if(key == 'q')
{
break;
}
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
// #1
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
fout1<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
Eigen::Matrix3f Rdelta = Rcurr.inverse() * lastPlaceRecognitionRot;
Eigen::Vector3f tdelta = tcurr - lastPlaceRecognitionTrans;
//Eigen::MatrixXd covariance = odom->getCovariance();
//Eigen::MatrixXd covariance=Eigen::Matrix<double, 6, 6>::Identity()* 1e-3;
if((Projection::rodrigues2(Rdelta).norm() + tdelta.norm()) >= 0.1)
{
Eigen::MatrixXd covariance = odom->getCovariance();
iSAM.addCameraCameraConstraint(lastTime,
logReader.timestamp,
lastPlaceRecognitionRot,
lastPlaceRecognitionTrans,
Rcurr,
tcurr);
//covariance);
printCovariance(fout5, covariance);
lastTime = logReader.timestamp;
lastPlaceRecognitionRot = Rcurr;
lastPlaceRecognitionTrans = tcurr;
cout<<"before add keyframe"<<endl;
// #2
map.addKeyframe((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
Rcurr,
tcurr,
logReader.timestamp);
fout2<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
/*
//Save keyframe
{
cv::Mat3b rgbImgKeyframe(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depthImgKeyframe(height, width, (unsigned short *)&decompressionBuffer[0]);
//save keyframe depth
char fileName[1024] = {NULL};
sprintf(fileName, "keyframe_depth_%06d.png", frame_index);
cv::imwrite(fileName, depthImgKeyframe);
//save keyframe rgb
sprintf(fileName, "keyframe_rgb_%06d.png", frame_index);
cv::imwrite(fileName, rgbImgKeyframe);
frame_index ++;
}
*/
int64_t matchTime;
Eigen::Matrix4d transformation;
// Eigen::MatrixXd cov(6,6);
//isam::Covariance(0.001 * Eigen::Matrix<double, 6, 6>::Identity()))
Eigen::MatrixXd cov=0.001 * Eigen::Matrix<double, 6, 6>::Identity();
cout<<"map.addKeyframe is OK"<<endl;
// #3
if(placeRecognition.detectLoop((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
logReader.timestamp,
matchTime,
transformation,
cov,
loopClosureCount))
{
//printCovariance(fout6, cov);
cout<<"logReader.timestamp "<<logReader.timestamp<<endl;
cout<<"matchTime "<<matchTime<<endl;
/*
transformation << -0.2913457145219732, 0.228056050293173, -0.9290361201559172, 2.799184934345601,
0.6790194052589797, 0.7333821627861707, -0.03291277242681545, 1.310438143604587,
0.673832562222562, -0.6404225489719699, -0.3685222338703895, 6.988973505496276,
0, 0, 0, 0.999999999999998;
*/
/*
transformation << 0.9998996846969838, 0.003948215234314986, -0.01360265192291004, 0.05847011404293689,
-0.004032877285312574, 0.9999726343121815, -0.006202138950136233, 0.04528938486109094,
0.01357779229749574, 0.006256374606648019, 0.9998882444218992, 0.02203456132723125,
0, 0, 0, 1;
*/
iSAM.addLoopConstraint(logReader.timestamp, matchTime, transformation);//, cov);
fout3<<transformation(0,0)<<" "<<transformation(0,1)<<" "<<transformation(0,2)<<" "<<transformation(0,3)<<" "<<transformation(1,0)<<" "<<transformation(1,1)<<" "<<transformation(1,2)<<" "<<transformation(1,3)<<" "<<transformation(2,0)<<" "<<transformation(2,1)<<" "<<transformation(2,2)<<" "<<transformation(2,3)<<" "<<transformation(3,0)<<" "<<transformation(3,1)<<" "<<transformation(3,2)<<" "<<transformation(3,3)<<endl;
loopClosureCount++;
}
}
if(loopClosureCount>=1)
{
break;
}
}
/*
for(int i=0; i<loopClosureCount;i++)
{
iSAM.addLoopConstraint(placeRecognition.loopClosureConstraints.at(i)->time1,
placeRecognition.loopClosureConstraints.at(i)->time2,
placeRecognition.loopClosureConstraints.at(i)->constraint);
}*/
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > posesBefore;
iSAM.getCameraPoses(posesBefore);
cout<<"It works good before optimization"<<endl;
// #4
double residual =iSAM.optimise();
cout<<"It works good after optimize and before map.applyPoses"<<endl;
// map.applyPoses(isam);
//cout<<"It works good before *cloud=map.getMap and after map.applyPoses(isam)"<<endl;
/*
pcl::PointCloud<pcl::PointXYZRGB> *cloud = map.getMap();
// Write it back to disk under a different name.
// Another possibility would be "savePCDFileBinary()".
cout<<"before storing the point cloud map"<<endl;
pcl::io::savePCDFileASCII ("outputCloudMap03DVODensity005.pcd", *cloud);
cout << "Saved data points to outputMap.pcd." << std::endl;
cout<<"copy data into octomap..."<<endl;
octomap::ColorOcTree tree( 0.05 );
for (size_t i=0; i<(*cloud).points.size(); i++)
{
// 将点云里的点插入到octomap中
tree.updateNode( octomap::point3d((*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z), true );
}
for (size_t i=0; i<(*cloud).points.size(); i++)
{
tree.integrateNodeColor( (*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z, (*cloud).points[i].r, (*cloud).points[i].g, (*cloud).points[i].b);
}
tree.updateInnerOccupancy();
tree.write("OctomapColorLab03DVODensity005.ot");
cout<<"please see the done."<<endl;
*/
//pcl::visualization::PCLVisualizer cloudViewer;
// cloudViewer.setBackgroundColor(1, 1, 1);
//cloudViewer.initCameraParameters();
// cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > newPoseGraph;
iSAM.getCameraPoses(newPoseGraph);
/*
for(unsigned int i = 0; i < newPoseGraph.size(); i++)
{
// file << std::setprecision(6) << std::fixed << (double)newPoseGraph.at(i).first / 1000000.0 << " ";
Eigen::Vector3f trans = newPoseGraph.at(i).second.topRightCorner(3, 1);
Eigen::Matrix3f rot = newPoseGraph.at(i).second.topLeftCorner(3, 3);
fout4 << trans(0) << " " << trans(1) << " " << trans(2) << " ";
Eigen::Quaternionf currentCameraRotation(rot);
//file << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";
}*/
for(std::vector<std::pair<uint64_t, Eigen::Matrix4f> >::iterator ite=newPoseGraph.begin(); ite!=newPoseGraph.end(); ite++)
{
Eigen::Matrix3f Roptimized;
Roptimized<<ite->second(0,0), ite->second(0,1), ite->second(0,2),
ite->second(1,0), ite->second(1,1), ite->second(1,2),
ite->second(2,0), ite->second(2,1), ite->second(2,2);
Eigen::Quaternionf quatOptimized(Roptimized);
fout4<<ite->second(0,3)<<" "<<ite->second(1,3)<<" "<<ite->second(2,3)<<" "<<quatOptimized.w()<<" "<<quatOptimized.x()<<" "<<quatOptimized.y()<<" "<<quatOptimized.z()<<endl;
}
cout<<"The number of optimized poses"<<newPoseGraph.size()<<endl;
// drawPoses(poses, cloudViewer, 1.0, 0, 0);
//drawPoses(posesBefore, cloudViewer, 0, 0, 1.0);
//cloudViewer.spin();
delete [] decompressionBuffer;
return 0;
}
