Fix offset calculation

Author: shengwen-tw

system/lane_detector.cpp

@@ -860,25 +860,30 @@ bool LaneDetector::generate_vote(segment_t& lane_segment, float& d,
 
 	Point2f n_hat(-t_hat.y, t_hat.x); //normal vector
 
-	float d1 = inner_product(n_hat, p1);
-	float d2 = inner_product(n_hat, p2);
-
-	d = (d1 + d2) / 2; //lateral displacement
+	Point2f offset_vector = n_hat;
 
 	if(color == WHITE) {
-		d -= W / 2;
-
 		if(lane_segment.side == RIGHT_EDGE) {
-			d -= L_W;
+			offset_vector *= -(W / 2) - L_W;
+		} else {
+			offset_vector *= -(W / 2);
 		}
 	} else if(color == YELLOW) {
-		d += W / 2;
-
 		if(lane_segment.side == LEFT_EDGE) {
-			d += L_Y;
+			offset_vector *= +(W / 2) + L_Y;
+		} else {
+			offset_vector *= +(W / 2);
 		}
 	}
 
+	p1 += offset_vector;
+	p2 += offset_vector;
+
+	float d1 = inner_product(n_hat, p1);
+	float d2 = inner_product(n_hat, p2);
+
+	d = (d1 + d2) / 2; //lateral displacement
+
 	//TODO:referive the geometry formulas!
 	d *= -1;