Skip to content

Commit

Permalink
recommit
Browse files Browse the repository at this point in the history
  • Loading branch information
root authored and root committed Mar 2, 2014
1 parent 328de2b commit 03ed89d
Show file tree
Hide file tree
Showing 12 changed files with 335 additions and 339 deletions.
Empty file modified CMakeLists.txt
100644 → 100755
View file
Empty file.
Empty file modified Makefile
100644 → 100755
View file
Empty file.
Empty file modified README
100644 → 100755
View file
Empty file.
Empty file modified demo_rgbd.launch
100644 → 100755
View file
Empty file.
Empty file modified demo_rgbd.vcg
100644 → 100755
View file
Empty file.
Empty file modified manifest.xml
100644 → 100755
View file
Empty file.
189 changes: 92 additions & 97 deletions src/bundleAdjust.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,109 +29,105 @@ pcl::PointCloud<DepthPoint>::Ptr depthPointsStacked(new pcl::PointCloud<DepthPoi
double depthPointsTime;
bool newKeyframe = false;

float rollRec, pitchRec, yawRec;
float txRec, tyRec, tzRec;
double rollRec, pitchRec, yawRec;
double txRec, tyRec, tzRec;

float transformBefBA[6] = {0};
float transformAftBA[6] = {0};
double transformBefBA[6] = {0};
double transformAftBA[6] = {0};

double robust_cost_function(double d) {
return cost_pseudo_huber(d, .5);
}

void diffRotation(float cx, float cy, float cz, float lx, float ly, float lz,
float &ox, float &oy, float &oz)
void diffRotation(double cx, double cy, double cz, double lx, double ly, double lz,
double &ox, double &oy, double &oz)
{
float srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx))
- cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx);
double srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx))
- cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx);
ox = -asin(srx);

float srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz))
- cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz);
float crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly);
double srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz))
- cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz);
double crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly);
oy = atan2(srycrx / cos(ox), crycrx / cos(ox));

float srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy)
- cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx))
- (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly));
float crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz)
+ cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz)
- cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz);
double srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy)
- cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx))
- (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly));
double crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz)
+ cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz)
- cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz);
oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
}

void transformAssociateToBA()
{
float txDiff = txRec - transformBefBA[3];
float tyDiff = tyRec - transformBefBA[4];
float tzDiff = tzRec - transformBefBA[5];
double txDiff = txRec - transformBefBA[3];
double tyDiff = tyRec - transformBefBA[4];
double tzDiff = tzRec - transformBefBA[5];

float x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff;
float y1 = tyDiff;
float z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff;
double x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff;
double y1 = tyDiff;
double z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff;

float x2 = x1;
float y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
float z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;
double x2 = x1;
double y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
double z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;

txDiff = cos(rollRec) * x2 + sin(rollRec) * y2;
tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2;
tzDiff = z2;

float sbcx = sin(pitchRec);
float cbcx = cos(pitchRec);
float sbcy = sin(yawRec);
float cbcy = cos(yawRec);
float sbcz = sin(rollRec);
float cbcz = cos(rollRec);

float sblx = sin(transformBefBA[0]);
float cblx = cos(transformBefBA[0]);
float sbly = sin(transformBefBA[1]);
float cbly = cos(transformBefBA[1]);
float sblz = sin(transformBefBA[2]);
float cblz = cos(transformBefBA[2]);

float salx = sin(transformAftBA[0]);
float calx = cos(transformAftBA[0]);
float saly = sin(transformAftBA[1]);
float caly = cos(transformAftBA[1]);
float salz = sin(transformAftBA[2]);
float calz = cos(transformAftBA[2]);

float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly)
- cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
- calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
- cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
- calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
double sbcx = sin(pitchRec);
double cbcx = cos(pitchRec);
double sbcy = sin(yawRec);
double cbcy = cos(yawRec);
double sbcz = sin(rollRec);
double cbcz = cos(rollRec);

double sblx = sin(transformBefBA[0]);
double cblx = cos(transformBefBA[0]);
double sbly = sin(transformBefBA[1]);
double cbly = cos(transformBefBA[1]);
double sblz = sin(transformBefBA[2]);
double cblz = cos(transformBefBA[2]);

double salx = sin(transformAftBA[0]);
double calx = cos(transformAftBA[0]);
double saly = sin(transformAftBA[1]);
double caly = cos(transformAftBA[1]);
double salz = sin(transformAftBA[2]);
double calz = cos(transformAftBA[2]);

double srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly)
- cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
- calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
- cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
- calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
pitchRec = -asin(srx);

float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
- calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
- calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
+ cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
- calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
- calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
+ cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
double srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
- calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
- calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
+ cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
double crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
- calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
- calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
+ cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
yawRec = atan2(srycrx / cos(pitchRec), crycrx / cos(pitchRec));

float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz)
- cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx)
- cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly)
+ (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx)
- calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz
+ sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz)
+ calx*cblx*salz*sblz);
float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly)
- cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx)
+ cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly)
+ calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly
- cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz)
- calx*calz*cblx*sblz);
double srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz)
- cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx)
- cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly)
+ (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx)
- calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz
+ sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz)
+ calx*cblx*salz*sblz);
double crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly)
- cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx)
+ cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly)
+ calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly
- cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz)
- calx*calz*cblx*sblz);
rollRec = atan2(srzcrx / cos(pitchRec), crzcrx / cos(pitchRec));

x1 = cos(rollRec) * txDiff - sin(rollRec) * tyDiff;
Expand Down Expand Up @@ -268,30 +264,30 @@ int main(int argc, char** argv)
poses.push_back(posen);
ba.add_node(posen);

float roll = depthPoints[i]->points[0].depth;
float pitch = depthPoints[i]->points[0].u;
float yaw = depthPoints[i]->points[0].v;
float tx = depthPoints[i]->points[1].u;
float ty = depthPoints[i]->points[1].v;
float tz = depthPoints[i]->points[1].depth;
double roll = depthPoints[i]->points[0].depth;
double pitch = depthPoints[i]->points[0].u;
double yaw = depthPoints[i]->points[0].v;
double tx = depthPoints[i]->points[1].u;
double ty = depthPoints[i]->points[1].v;
double tz = depthPoints[i]->points[1].depth;

float txDiff = tx - txRec;
float tyDiff = ty - tyRec;
float tzDiff = tz - tzRec;
double txDiff = tx - txRec;
double tyDiff = ty - tyRec;
double tzDiff = tz - tzRec;

float x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff;
float y1 = tyDiff;
float z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff;
double x1 = cos(yawRec) * txDiff - sin(yawRec) * tzDiff;
double y1 = tyDiff;
double z1 = sin(yawRec) * txDiff + cos(yawRec) * tzDiff;

float x2 = x1;
float y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
float z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;
double x2 = x1;
double y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
double z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;

txDiff = cos(rollRec) * x2 + sin(rollRec) * y2;
tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2;
tzDiff = z2;

float rollDiff, pitchDiff, yawDiff;
double rollDiff, pitchDiff, yawDiff;
diffRotation(pitch, yaw, roll, pitchRec, yawRec, rollRec, pitchDiff, yawDiff, rollDiff);

Pose3d_Pose3d_Factor* poseposeFactorn = new Pose3d_Pose3d_Factor
Expand Down Expand Up @@ -466,7 +462,6 @@ int main(int argc, char** argv)
Properties prop = ba.properties();
prop.method = DOG_LEG;
ba.set_properties(prop);
//ba.set_cost_function(robust_cost_function);
ba.batch_optimization();

transformBefBA[0] = depthPoints[keyframeNum - 1]->points[0].u;
Expand Down
22 changes: 11 additions & 11 deletions src/processDepthmap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloudArray[keepSyncCloudNum];
int syncCloudInd = -1;
int cloudRegInd = 0;

int cloudCount = -1;
const int cloudSkipNum = 5;

pcl::PointCloud<pcl::PointXYZI>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud2(new pcl::PointCloud<pcl::PointXYZI>());
Expand All @@ -34,6 +31,9 @@ double timeRec = 0;
double rxRec = 0, ryRec = 0, rzRec = 0;
double txRec = 0, tyRec = 0, tzRec = 0;

int cloudCount = -1;
const int cloudSkipNum = 5;

ros::Publisher *depthCloudPubPointer = NULL;

void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
Expand Down Expand Up @@ -66,13 +66,13 @@ void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
tyRec = voData->pose.pose.position.y;
tzRec = voData->pose.pose.position.z;

float x1 = cos(yaw) * tx + sin(yaw) * tz;
float y1 = ty;
float z1 = -sin(yaw) * tx + cos(yaw) * tz;
double x1 = cos(yaw) * tx + sin(yaw) * tz;
double y1 = ty;
double z1 = -sin(yaw) * tx + cos(yaw) * tz;

float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
double x2 = x1;
double y2 = cos(pitch) * y1 - sin(pitch) * z1;
double z2 = sin(pitch) * y1 + cos(pitch) * z1;

tx = cos(roll) * x2 + sin(roll) * y2;
ty = -sin(roll) * x2 + cos(roll) * y2;
Expand Down Expand Up @@ -227,8 +227,8 @@ void syncCloudHandler(const sensor_msgs::Image::ConstPtr& syncCloud2)
int xd = i % imageWidth;
int yd = int(i / imageWidth);

float ud = (kDepth[2] - xd) / kDepth[0];
float vd = (kDepth[5] - yd) / kDepth[4];
double ud = (kDepth[2] - xd) / kDepth[0];
double vd = (kDepth[5] - yd) / kDepth[4];

point.z = val;
point.x = ud * val;
Expand Down
Loading

0 comments on commit 03ed89d

Please sign in to comment.